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.  INTRODUCTION 


The  objective  of  this  task  was  to  develop  and  validate 
control  algorithms  for  the  dual-arm  robotic  manipulation  of  a 
single  object. ^This  objective  has  been  achieved. 


■The  specific  tasks  performed  to  attain  the  objective 


were : 


Prepare  control  boards  suitable  for  the  processing  and 
data  handling  necessary  for  the  dual-arm  robotic 
manipulation  of  a  single  'object'  'using-  vrist  mounted 
force/torque  sensors  as  feedback  devices^ 

'i) 

?  Devise  and  implement  a  means  for  estimating  the  weight 

1  of  the  gripped  object  and  eliminating  that  reaction  from 

l 

the  individual  force/torque  transducer  readings^ 

3) 

y  Implement  an  interface  with  an  external  vision  system 
that  is  to  be  used  to  locate  the  initial  position  and 
orientation  of  the  object  to  be  manipulated  and,  using 
that  input,  individually  guide  each  arm  to  an  initial 
grip  point' 

y  $  Implement  a  technique  for  removing  any  initial 
misalignment  existing  between  either  gripper  and  the 
gripped  object j 

y  ^Implement  a  control  technique  to  allow  the  trajectory 
of  one  of  the  cooperating  manipulators  to  adapt  to  any 
misalignment  error  occuring  during  the  dual-arm 

manipulation  of  the  object j 


/-  ^Validate  the  control  algorithm  and  develop  a  working 
demonstration  of  the  control  approach  j 

n )  f),  n 


Document  all  control  software  and  test  results. 
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II.  BACKGROUND 


In  some  military  applications  using  automation  it  may  be 
necessary  to  manipulate  a  single  object  using  multiple  robot 
arms-  This  will  occur  when  the  weight  of  the  object  exceeds 
the  payload  of  a  single  arm  or  when  a  relatively  large 
component  of  a  weapons  system  or  structure  must  be  positioned 
with  great  accuracy. 

Svern  and  Tricamo  have  published  a  number  of  works 
dealing  with  two-arm  robotic  manipulation  and  control 
Cl]— 19J .  The  work  detailed  in  this  report  represents  an 
extension  and  development  of  many  of  the  concepts  embodied  in 
these  publications. 

The  approach  offers  a  number  of  advantages/  including 
the  following: 

-  the  capability  of  operating  each  robot  autonomously  in 
the  event  that  loss  of  grip  or  slippage  occurs  at  the 
individual  robot  grip  points  on  the  object, 

-  the  capability  of  sensing  contact  between  the  object 
and  the  environment, 

the  development  of  an  interface  with  an  external 
vision  system  capable  of  providing  the  location  of  the 
object  to  be  manipulated,  and 

the  capability  being  modified  to  allow  for  the 
teach-mode  operation  of  the  dual-arm  manipulators. 

Other  authors  have  also  dealt  with  various  aspects  of 
the  control  of  cooperating  robots  [10]— [14]  and  similar 
closed-chain  configurations. 
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III.  TECHNICAL  DISCUSSION 


The  control  approach  required  for  multi-arm  robotic 
manipulation  differs  significantly  from  that  used  in 
autonomous  operation.  When  two  arms  grasp  a  single  object, 
the  resulting  structure  forms  a  closed  kinematic  chain.  The 
number  of  e\ctuators  in  this  new  structure  is  greater  than  the 
minimum  needed  to  position  the  object.  This  redundancy  can 
result  in  the  creation  of  constraint  forces  within  the  object 
and  manipulators.  Such  a  condition  is  brought  about  when 
position  or  orientation  errors  caused  by  manipulator 
miscalibration,  grip  point  slippage,  or  similar  effects  cause 
interaction  between  the  various  actuators  in  each  arm.  These 
forces  may  be  of  sufficient  magnitude  to  make  accurate 
positioning  of  the  object  difficult  to  achieve. 

The  approach  implemented  in  the  present  work  to  achieve 
multi-arm  cooperative  robotic  control  is  based  upon  the  use 
of  force/torque  transducers  to  measure  constraint  forces  in 
the  object  and  manipulator  system  brought  about  by  trajectory 
inconsistencies  between  individual  robots.  These  constraint 
reaction  measurements  are  used  to  determine  the  magnitude  of 
position  and  orientation  errors  existing  between  the  grippers 
of  the  cooperating  arms.  Evaluation  of  these  errors  provides 
the  basis  for  a  compensation  sheme  to  control  the 
coordination  of  the  two  arms. 

A  detailed  description  of  the  development, 
implementation,  and  testing  of  the  control  algorithm  used  to 
perform  the  tasks  outline  in  section  I  of  this  report  is 
given  below. 

A.  Robot  Configuration 

1.  Kinematic  Model 

A  schematic  of  the  two-arm  configuration  is  shown  in 
Figure  III-l.  The  world  coordinate  reference  frames  of  each 
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SCHEMATIC  OF  TWO-ARM 


CONFIG 


robot  are  aligned  with  one  another  and  their  origins  are  36.0 
inches  apart.  The  origin  of  the  world  reference  frame  for  the 
muliti-robot  configuration  is  taken  as  coincident  with  that 
of  robot  2.  The  coordinate  axes  of  the  wrist  mounted 
force/torque  transducers  each  has  a  different  degree  of 
misalignment  with  respect  to  their  corresponding  end  effector 
frames.  This  misalignment  is  compensated  for  by  appropriate 
transformations,  as  shown  below. 

As  shown  in  the  figure,  the  following  transformations 
are  defined  for  each  of  robots  1  and  2  as  denoted  by  the 
appropriate  subscripts: 


[  Tx  J ,  £  T2 1 


transformation  from  base  frame  to  end 
effector  frame 


l ] r  E  E2 ]  =  transformation  from  end  effector  frame 
to  force/torque  transducer  frame 

f ] , £ G2 ]  =  transformation  from  force/torque  transducer 
frame  to  gripper  frame 


[Hj^J  =  transformation  between  frames  of  grippers 
1  and  2,  respectively 

£ B2i ]  =  transformation  between  individual  world 
frames  for  robots  2  and  1,  respectively 


Assuming  perfect  grip ,  the  transformations  are  related 
as  follows: 


(b21](t1he1hg1][h12]  =  £  t2  ]  [  e2  )  [g2  ]  (1) 

where  transforms  [B21b  £  ] ,  E2),  and  [G2)  are  known 
and  constant.  Transforms  IT^]  and  (T2)  vary  as  each  arm 
moves.  The  elements  of  the  latter  transforms  are  available 
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as  output  from  the  VAL  II  software  ip  each  fob.pt  ponfrollgr. 
Determination  of  the  remaining  transform,  the 

subject  cf  the  next  section. 

2.  Fprce/Tprgqe  Model 

Transform  tB12l  coMld  be  fpxmf|  using  equation  (1)  if 
there  were  np  kinematic  errors  in  either  robot  and  if  the 
o.hjecf  was  Perfectly  located  in  each  grippgr.  However, 
npff^er  of  these  conditions  is  present  and  the  determination 
of  fhe  relative  positioning  of  each  gripper  must  pg  ra^cte  by 
cfhef  means. 

This  was  accomplished  using  the  forcg/forque  sensor 
readings.  pue  to  tbe  fact  that  grip  in  the  vertical 
direction  (woffd  z-axis)  was  lopse,  leading  fo  inaccurate 
z-force  and  x-moment  readings,  it  was  decided  fo  manipulate 
the  ofc>ject  ih  a  plane  parallel  to  the  X-Y  world  plane.  This 
sifiplif leaf ign  in  no  way  limifs  the  applicablilify  of  the 
devised  confrpl  technique. 

A  schematic  of  the  pbjepf  in  grip  is  show?)  in  Figure 
III-2.  Assuming,  for  simplif ipafion t  that  the  object  ip  grip 
and  the  manipulator  linkages  are  rigid,  file  compliance 
between  fhe  grip  surfaces  and  the  pbjepf  may  bg  modeled  as 
parallel  springs,  as  shown  in  Figure  III-3.  The  components 
drawn  in  splid  lines  represent  fhe  apfual  positions  pf  the 
gripper  and  pbject  while  the  dashed  reference  line  reppesenfg 
a  datum  for  measuring  the  actual  spring  displacements.  Only 
fhe  gripper  and  pbjept  pf  robot  1  are  shown;  fhat  of  rpbPt  2 
would  be  similar  in  appearance. 


In  Figure  III-3,  the  following  nomenclature  is  used: 

xgi f  xpi  =  PPsibion  displacement  af  spring  end 
of  gripper  and  part  (object)/ 

(i=l,2)  respectively,  as  measured  ffpifi  the 
lapdef lected  position  of  thg  spfing 
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Assuming  the  same  spring  constant,  k,  for  each 
contacting  surface,  the  forces  developed  at  the  attachment 
points  for  each  spring  are  given  by: 

(2) 

(3) 

(4) 

(5) 

The  force  and  torque  developed  on  the  gripper  are: 


gl  =  k|xgi  "  V 

£g2  -  k(xg2  “  xp2> 
£pl  -  k|xpl  -  xgl> 
fp2  ‘  k(xp2  -  xg2 > 
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(11) 


«pl  *■  <*pl  +  *p2)/? 


-  dgl  -  Fgl/2k 


(12) 


0pl  s  Up2  -  xpl'/2s 

-  v  -  v/2ks2  ,13) 

where  equations  (12)  and  (13)  were  obtained  with  the  help  of 
equations  ( 2 ) , ( 3 ) , ( 6  ) , ( 7  ) ,  ( 10 ,  and  (11). 

Equations  (10)-(13)  may  be  expressed  in  the  following 
matrix  form: 


V 

-1  0  0  0*" 

rgi 

Tpl 

as 

0-100 

Tgi 

dpl 

-l/2k  010 

dgi 

-V- 

0  -l/2ks2  0  1 

i 

H 

cn 

_^_j 

In  a  similar  manner,  the  following  matrix  expression  may 
be  obtained  for  the  gripper  of  robot  2: 


-1 


0  0 


0 


P  -1  0 


0 


(15) 
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-l/2k  0 


1 


0 


0  -l/2ks2  0 


1 


For  the  rigid  bar,  as  shown  in  Figure  III-4,  the 
following  matrix  expression  relating  quantities  at  robot  1 
and  2  grip  points  may  be  determined: 


Fp2 

-10  0  0 

Fpl 

Tp2 

-L  -1  0  0 

Tpl 

d  0 

0  0  1  -L 

d  , 

p2 

pi 

®p2__ 

, 

0  0  0  1 

-V 

(16) 


In  the  above,  it  should  be  noted  that  if  a  flexible  object 
were  gripped  equation  (16)  would  have  to  be  modified  to 
include  the  elastic  properties  of  the  object.  The  overall 
approach,  however,  would  remain  the  same. 


Combining  expressions  (14),  (15),  and  (16)  results  in  the 
following  relationship  between  reactions  and  displacements 
for  the  two  grippers: 


Fg2 

-10  0  0 

Fgi 

Tg2 

-L  -1  0  0 

Tgi 

dg2 

-1/k  -L/2ks2  1  -L 

dgi 

0g2 

-L/2ks2  -1/ks2  0  1 

G9i 

_  — 

V 

(17) 
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To  determine  the  relative  linear  and 
displacements  of  gripper  2  (slave)  with  respect  to 
(master),  let  dgi=0gi=()  in  expression  (i7).  Using 
two  lines  of  this  same  expression,  the  following  is 


angular 
gripper  1 
the  first 
obtained: 


—  — 

n  “ 

—  — 

dg2 

=  l/2ks2 

-( L2-2s2 )  L 

Fg2  . 

8g2 

-L  2 

Tg2 

(18) 


Expression  (18)  then  gives  a  relationship  between  the  sensed 
force  and  torque  reactions  at  the  slave  gripper  due  to  the 
misalignment  between  the  grippers  and  the  corresponding 
linear  and  angular  displacements  required  to  eliminate  that 
misalignment.  As  such,  it  represents  the  basis  for  the 
control  approach.  By  repeated  sampling  of  the  force/torque 
reactions,  corrections  as  defined  by  expression  (18)  are  made 
to  the  location  and  orientation  of  the  slave  gripper. 
Previous  work  has  shown  (see  reference  [7])  that  even  in  the 
presence  of  noise  and  other  errors  in  the  sensed  reactions  or 
physical  parameters  the  control  approach  is  stable  and  will 
result  in  eliminating  the  misalignment  over  relatively  few 
iterations . 


B.  Control  Approach 

1.  Hardware  Configuration 

The  force/torque  readings  were  obtained  using  two  LORD 
Model  15/50  transducers.  This  device  has  a  parallel  port 
available  that  transmits  three  force  and  three  torque 
readings  as  a  single  group  at  rates  up  to  100  groups  per 
second.  The  transducer  also  has  a  language  that  can  be  used 
to  perform  simple  thresholding  operations  on  its  readings, 
and  to  logically  combine  these  discrete  outputs. 
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Referring  to  Figure  III-5,  the  central  component  of  the 
control  system  is  an  INTEL  8614  single  board  processor 
ccri^a^ping  an  8Q86  processor.  Thi§  processor  is  used  as  a 
dis.crete  controller  with  an  iteration  time  of  28  msec  to 
m^tch  that  of  the  EIJMA  660,.  T^e  controlling  INTEL  8614 
prqcegsor  is  connected  to  a  second  bq^rd,  serving  as  an 
?/Q  controller,  via  a  Multi-Bits  J.  The  I/p  controller 
contains  sufficient  parallel  and  serial  ports  to,  interface 
both  fprce/tprque  sensors  as  well  as  the  YAL  II  ALTER 
interface . 

In  the  development  of  the  control  strategy,  it  was  noted 
that  the  iteration  tin>e  of  the  controller  is  fixed  by  the 
availability  of  force/torque  data  from  the  transducers  and 
the  ability  of  the  VAL  processor  to  accept  and  execute 
movement  commands.  Figure  III-6  sh°VS  the  iteration  time  of 
the  various  loops  involved,  including  the  loops  internal  to 
tfte  robpt  controllers. 

In  besting  the  system,  it  was  further  noted  that 
performance  was  limited  by  the  processing  capability  of  the 
8086  processor. 

2.  Gravity  Compensation 


An  advantage  gained  in  the  use  of  two  force/torque 
transducers  was  the  ability  tp  easily  measure  external 
reactions  acting  on  the  manipulated  object  by  simply  summing 
the  corresponding  readings  of  each  platform.  In  the  absence 
qf  external  reactiqps,  the  combined  readings  on  all  axes 
would  be  zero,  indicating  the  presence  of  only  constraint 
reactions.  Finite  sums  indicate  the  presence  qf  an  external 
force  or  torque. 


The  only  external  reaction  dealt  with  in  the  present 
application  was  that  due  to  gravity.  The  algorithm 
calculates  the  weight  of  the  object  as  indicated  above.  Care 
must  be  taken  ip  evaluating  this  quantity  to  be  certain  that 
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FIGURE  III— 5 :  HARDWARE  INTERCONNECTION  DIAGRAM 
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ROBOT  I  ROBOT  II 


FIGURE  III-6:  HARDWARE  TIMING  DIAGRAM 
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all  readings  are  transformed  to  a  common  reference  frame 
before  summation  is  undertaken.  After  the  weight  measurement 
is  taken,  it  is  assumed  that  the  weight  is  equally 
distributed  between  the  two  grippers  and  an  appropriate 
correction  is  made  to  individual  sensor  readings  to  bias  off 
this  gravity  effect,  thereby  leaving  only  constraint 
reactions  readings  for  use  in  correcting  misalignments. 

3.  Rate  Filter 

To  avoid  sudden  movements  due  to  erroneous  force/torque 
readings  due  to  noise  or  other  effects,  a  rate  filter  was 
employed  to  condition  the  input  data.  This  was  accomplished 
by  averaging  the  sensor  readings  on  each  channel  over  8 
samples  as  the  iteration  scheme  progressed.  This  technique 
worked  well,  but  it  did  result  in  some  degradation  of 
performance  due  to  the  slower  response  inherent  with  this 
approach. 

C.  Interface  to  Vision  System 

The  control  algorithm  provides  a  means  of  using  an 
external  vision  system  to  locate  and  transmit  the  initial 
grip  point  locations  and  orientations  for  the  two 
manipulators . 

If  both  robots  have  not  reached  their  grip  point,  then 
the  controller  sends  back  position  corrections  only  based  on 
vision  system  input.  After  both  robots  reach  their  grip 
point,  both  navigational  and  constraint  based  feedback  may 
occur.  Coordinate  changes  due  to  vision  system  input  must  be 
removed  for  the  robot  to  reach  the  target  location.  After 
navigation  is  complete,  force  feedback  is  implemented. 
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iv.  Summary  6f  results 


Th§  control  algorithms  were  validated  using  the 
following  sequence  of  operations  for  th§  duai-arm 
Configuration: 

ii  Each  robot  atm  moves  to  a  preset  location  tinder 
autonomous  operation 

2;  A  target  grip  point  is  computed  for  each  mahipulator 
usiftg  inptit  data  frofn  the  vision  system 

3i  Each  arm  is  commanded  to  mbve  to  the  target  grip 
point 

4.  Each  gripper  doses  to  grasp  the  object 

5j  A  path  is  computed  back  to  the  positions  taught  in 
step  1. 


6.  both  aims  move  to  target  position  as  force/tbirque 
algorithm  refhovgs  any  misalignment  ih  grippers 

7.  With  both  grippers  rtow  aligned,  Coordinated  movement 
to  hew  location  i§  accomplished  under  force/torque 
Control 


The  test  sequ§nc§  was  successfully  executed  many  times  Usihg 
objects  of  differing  weight  and  stiffness;  Thfe  contrbi 

algorithms  functioned  as  intended  in  each  case; 
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V.  CONCLUSIONS  AND  RECOMMENDATIONS  FOR  FUTURE  WORK 

A  control  algorithm  and  corresponding  hardware  to 
accomplish  the  dual-arm  robotic  manipulation  of  a  single 
object  was  devised,  implemented,  and  successfully  tested. 
The  algorithm  uses  input  from  wrist  mounted  force/torque 
transducers  to  sense  and  correct  for  any  misalignment  or 
trajectory  inconsistencies  that  may  develop  during  the 
cooperative  manipulation  of  the  test  object.  The  control 
algorithm  is  capable  of  taking  in  data  from  an  external 
vision  system  to  guide  each  robot  arm  to  its  initial  grip 
position. 

The  existing  system  represents  one  of  the  few  dual-arm 
robotic  test  facilities  in  the  nation.  To  further  improve 
and  extend  its  performance  capabilities,  the  following 
suggestions  are  made  for  future  work: 

-  upgrade  the  processor  in  the  control  system  to  a  386 

-  interface  the  control  system  to  Matrix  AR 

-  develop  a  dual-arm  teach  mode  capability 

evaluate  the  possibility  of  attaining  significant 
payload  increases  using  the  inherent  strength  of  the 
dual-arm  closed-chain  configuration 

incorporate  a  dual-arm  control  technique  capable  of 
using  external  force  signals  to  accomplish  a  given 
objective  (assembling  parts,  for  example) 

evaluate  the  advantage  and  feasibility  of  multi-arm 
manipulation  using  more  than  two  manipulators. 
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FORTRAN-86  COMPILER 
CONTRL. FOR 

DOS  3.30  (038-N)  FORTRAN-86  COMPILER  V3 . 0 
COMPILER  INVOKED  BY:  C:\INTEL\FORT86.EXE  CONTRL. FOR 

1  BLOCK  DATA 

C 

C  NAME:  Block  Data 

C 

C  PURPOSE:  Initialize  Common  Blocks 

c 

C  DESCRIPITON:  This  module  contains  values  of  constants  used  in  other 
C  FORTRAN  modules  in  the  program: 

c 

c 

C  Main  Common  Variables  are  set  to  zero  to  insure  that  the  controller 
C  starts  from  some  known  state. 

c 

$ INCLUDE  (FTORQUE. INC) 

=1  $NOLIST 

2=1  INTEGER  FTMFLAG , FTDOIT , FTDONE , FSTART, FTMFLAG2 , ROBDIF1 , ROBDIF2 

3=1  INTEGER  FTGRIP , R2MSG , GRIPPD , STARTD , R1MSG , FCALC , AUTO , FTAUTO 

4=1  INTEGER  MVSKED , MVBAK , MVRAK 

5=1  CHARACTER*  4  0  PROMPT 

6=1  CHARACTER* 3  DISPT 

7=1  LOGICAL* 1  RESET, FTPLOT 

=1  C 

8=1  PARAMETER  (R2MSG=2 , R1MSG=1) 

9=1  PARAMETER  (AUT0=34 , GRIPPD=20 , STARTD=65) 

10=1  PARAMETER  (FSTART=1 , FTAUT0=2 , FTGRIP=4 , FTD0NE=8 ) 

11=1  PARAMETER  (FTDOIT=l6, NAVSW=32 ,MOVEDONE=64 , FCALC=128) 

12=1  PARAMETER  (MVSKED=1,MVBAK=2,MVRAK=4) 

=1  C 

13=1  COMMON/FTCOM/FORCE ( 14 ) , FTBIAS ( 14 ) , FTDIF ( 14 ) , FSCALE , TSCALE , TDZ 

14=1  COMMON/FTSUM/F1ZSUM ( 6 )  ,F2ZSUM(6)  ,FZSUM(6)  , FSUM(6) 

15=1  COMMON/FTMAT/XGAIN,YGAIN,ZGAIN, ALENTH,FTMAT2 (6,6) , 

=1  1  FTMATX (6,6) , RSCALE ( 6 ) 

16=1  COMMON/XFORM/CTTOW1 (3,3) ,CTTOW2(3,3) ,GTOOL2(6,6) , 

=1  4  FTOOL1 ( 3 ) , FT00L2 ( 3 ) , MT00L1 ( 3 ) , MT00L2 ( 3 ) , GTOOL1 (3,3) 

17=1  COMMON/VECT/DX, DY , DZ , DX2 , DY2 , PHIZ 2 , DPHIM 

18=1  COMMON/MSGCOM/ PROMPT , RESET , FTPLOT (28) 

19=1  COMMON/MCNTL/ IOUTER , I FLAG , DI S  PT , FTMFLAG , FTMFLAG 2 

20=1  COMMON/NAVECT/BX , BY , BZ , RACKPT (4,2) ,ROBPTl(6) ,ROBPT2(6) 

21=1  COMMON/NAVEPT/ROBDIF1 (6) ,R0BDIF2(6) ,ROBWDIFl(6) ,ROBWDIF2(6) , 

=1  1  RDMAX , RTMAX , NCALC 

C 

22  DATA  FORCE, FTBIAS , FTDIF/42*0 . 0/ 

23  DATA  RSCALE/32. 0,32. 0,32. 0,182. 0444, 182. 0444, 182. 0444/ 

24  DATA  FTMFLAG, FTMFLAG2/ 2*0/ 

25  DATA  FTMAT2/3 6*0.0/ 

26  DATA  PROMPT/ 'ENTER  COMMAND'/ 

C 

27  END 
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FORTRAN-8 £  COMPILER 
CONTRL.FpR 


1  SUBROUTINE  CINIT(I, IN) 

P 

C  NAME:  Constant  Initialization 

c 

C  PURPOSE:  Initialize  Constants  in  the  FORTRJ^N  programs 

c 

Q  DESCRIPTION:  This  prpgram  allows  certain  constants  to  be  set  in  memor 
C  and  then  moved  oyer  to  bp  vised  by  the  FORTRAN  subroutines.  It  is 

C  easier  to  change  these' constants j  using  the  debug  monitor,  on  the 

C  fly  rather  than  recompile  the  FORTRAN  program. 

c 

2  INTE<3ER*2  IN,  I 
$INCLUDE  (FTORQUE.INC) 

=1  $NOLIST 

3=1  INTEGER  FTMFLAG, FTDOIT, FTDONE, FSTART, FTMFLAG2 ,ROBDIFl,ROBDIF2 

4=1  INTEGER  FTGRIP, R2MSG , GRIPPD ,  STARTD, R1MSG , FCALC, AUTO , FTAUTO 

5=1  INTEGER  MVSKED , MVBAK , MVRAK 

6=1  CHARACTER*  4  6  PROMPT 

7=1  CHARACTER* 3  DISPT 

8=1  LOGICAL* 1  RESET, FTPLOT 

=1  C 

9=1  PARAMETER  (R2MSG=2  ,R1MSG=1) 

10=1  PARAMETER  (AUTO=34,GRIPPD=20,STARTD=65) 

11=1  PARAMETER  { FSTART=1 , FTAUTO=2 , FTGRIP=4 , FTDONE=8 ) 

12=1  PARAMETER  (FTDOIT=i6,NAVSW=32,MOVEDONE=64,FCALC=128) 

13=1  PARAMETER  (MVSKED=1,MVBAK=2,MVRAK=4  j 

=1  C 

14=1  COMMON/FTCOM/FORCE ( 14 ) ,  FTBIAS ( 14 ) , FTDIF ( 14 ) , FSCALE , TSCALE , TDZ 

15=1  COMMON/FTSUM/FiZSUM (6)  ,F2ZSUN(6)  ,FZSUM(6)  ,FSUM(6) 

16=1  GOMMON/FTMAT/XGAIN ,  YGAIN ,  ZGAIN , ALENTH , FTMAT2  (6,6) , 

=1  1  FTMATX (6,6) , RSCALE ( 6 ) 

17=1  COMMON/XFORM/CTTOH1 (3,3),  CTTOW2 (3,3), GTOOL2  (6,6), 

=1  4  FTOOL1  ( 3 ) ,  FTOOL2  ( 3 )  ,  MTOOL1  ( 3 )  ,  MTOOL2  ( 3 )  ,  GTOOL1  (3,3) 

18=1  COMMON/ VECT/DX, DY, DZ , DX2 ,  DY2 , PHIZ2 , DPHIM 

19=1  COMMON/MSGCOM/PRONPT , RESET , FTPLOT (28) 

20=1  COMMON/MONTL/ 1  OUTER ,  IFLAG,  DISPT,  FTMFLAG ,  FTMFLAG2 

21=1  COMMON/NAVECT/BX , BY ,  BZ ,  RACKPT (4,2), ROBPT1 ( 6  j , ROBPT2 ( 6 ) 

22=1  COMMON/NAYEPT/ROBblFi (6)  ,ROBDIF2 (6) ,ROBWDIFl(6) ,ROBWDIF2(6) , 

=1  1  RDMAX , RTMAX , NCALC 

23  IOUTER=0 
C 

C  This  call  sets  the  words  "REINIT"  on  the  monitor  screen  to  verify  tha 
C  the  RS232  interface  to  that  screen  is  operational 
C 

24  CALL  UISPCH( ’REINIT’) 

C 

C  To  set  the  Debug  print  flag 

25  IF(I.EQ.3)  FTPFLAG=IN 

C 

C  Scaling  constants  for  the  force/torque  algorithms 
C 

26  FSCALE=1. 0 

27  TSGALE=1. 0 
C 

C  Set  force/torque  matrix  gains 

c 
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28  XGAIN=. 0075 

29  YGAIN=. 0004 

30  ZGAIN=. 0030 

31  ALENTH=11. 0 

32  CALL  GAINM 
C 

C  Rate  Constants  used  for  the  algorithms 

c 

33  DPHIM  =  .02 

34  RDMAX  =5.0 

35  RTMAX  =2.0 

C 

C  Set  the  initialization  flag  and  other  flags 
C 

36  FTMFLAG=0 

37  FTMFLAG2=0 

38  NCALC=0 
C 

C  Clear  arrays  and  data  areas  to  prevent  startup  transients 
C 

39  DO  10  J=l,6 

40  ROBPT1 ( J) =0 . 0 

41  R0BPT2 ( J) =0 . 0 

42  R0BWDIF1 ( J) =0 , 0 

43  ROBWDIF2 ( J) =0 . 0 

44  R0BDIF1 ( J) =0 

45  10  ROBDIF2 ( J) =0 

C 

c  Reset  grip  angle  to  zero 
C 

46  PHIZ2=0 . 0 

47  CALL  TOOLT2 (0.0,0. 0,-1) 

C 

C  Set  base  offset  of  two  robots 
C 

48  BX=-3  6 . 0 

49  BY=0 . 0 

50  BZ=0 . 0 

51  RETURN 

52  END 
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1  SUBROUTINE  CKFTF(I,iFTIN) 

c 

C  NAME:  Check  Force/Torque  Data 

c 

C  PURPOSE:  Preprocess  data  from  each  axis  of  Force/Torque  Sensor 
C 

C  DESCRIPTION:  The  raw  Force/Torque  data  is  corrected  for  sensed 
C  gravity; 

C 

C  iMPUTS:  I  -  Foirce/Torque  sexisor  axig  identification  number 

C  IFTIN  -  Foirce/Torque  sensor  irekding 

C 

2  INTEC-.ER*2  I,IFTIN,FTID(14) 

3  REAL  VFIL,DOBIAS 

4  DATA  FTID/1, 2 /3,3*4;0,l,5,6,3*4/0/ 

C 

$INCLUDE  (FTORQUE. INC) 

=1  $NOLIST 

5=1  INTEGER  FTMFLAG, FTDOIT, FTDONE, FSTART, FTMFLAG2 ,R0BDIF1,R0BDIF2 

6=1  INTEGER  FTGRIP,R2MSG, GRIPPD, STARTD,RlMSG, FCALC, AUTO, FTAUTO 

7=1  Integer  mvsked , mvbak , mvrak 

8=1  CHARACTER*  4  0  PROMPT 

9=1  CHARACTER* 3  DISPT 

10=1  LOGICAL* 1  RESET, FTPLOT 

=1  C 

11=1  PARAMETER  (R2MSG=2 , R1MSG=1) 

12=1  PARAMETER  (AUTO=34,GR±PPD=20,STARTD=65) 

13=1  PARAMETER  ( FSTART=1 , FTAUT0=2 , FTGRIP=4 , FTDONE=8 ) 

14=1  PARAMETER  (FTDOIT=16,NAVSW=32,MOVEDONE=64>FCALC=128) 

15=1  PARAMETER  (MVSKED=i , MVBAK=2 , MVRAK=4 ) 

=1  C 

16=1  COMMON/FTCOM/FORCE ( 14 ) , FTBIAS ( 14 ) , FTDIF ( 14 ) , FSCALE , TSCALE , TDZ 

17=1  COMMON/FTSUM/F1ZSUM ( 6 )  ,F2ZEUM(6)  > FZSUM(6)  , FSTJM(6) 

18=1  COMMON/FTMAT/XGAIN, YGAIN, ZGAIN> ALENTH, FTMAT2 (6,6) , 

=1  1  FTMATX (6,6) , RSCALE ( 6 ) 

19=1  COMMON/XFORM/CTTOW1 (3,3) ,CTTOW2(3,3) ,GTOOL2(6,6) , 

=1  4  FTOOL1 ( 3 ) , FTOOL2 ( 3 ) , MTOOL1 ( 3 ) , MTO0L2 ( 3 ) , GTOOL1 (3,3) 

20=1  COMMoN/VECT/DX, DY , DZ , DX2 , DY2 , PHIZ2  >  DEHIM 

2 1=1  COMMON/MSGCOM/ PROMPT , RESET , FTPLOT (28) 

22=1  COMmON/HCNTL/IOUTER, IFLAG , DISPT , FTMFLAG , FTMFLAG2 

23=1  COMMON/NAVECT/BX,BY,BZ,RACKPT(4,2) ,ROBPTl(6) ,ROBPT2(6) 

24=1  COMMON/NAVEPT/ROBDIF1 (6) ,ROBDIF2(6) ,ROBWDIFl(6) ,ROBWDIF2(6) , 

=1  1  RDMAX , RTMAX , NCALC 

C 

25  JFTIN=IFTIN 

26  FRIN=JFTIN 

27  GOTO  (1,2, 3, 4, 6, 7) , FTID(I) 

C 

28  1  FORCE ( I ) =FRIN 

29  GOTO  5 
C 

30  2  FORCE (I) =FRIN+TDZ*FORCE (1+4 ) 

31  GOTO  5 
C 

32  3  FORCE (I) =FRIN-TDZ*FORCE (1+2 ) 

33  GOTO  5 
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C 


34 

4 

FORCE ( I ) =FRIN 

35 

C 

GOTO  5 

36 

6 

FORCE ( I ) =FRIN-TDZ  * FORCE ( 1+4 ) 

37 

C 

GOTO  5 

38 

7 

FORCE (I) =FRIN+TDZ*FORCE (1+2 ) 

39 

GOTO  5 

C 

C  Calculate  the  Gravity  compensated  force  (FTDIF) 
C 


40  5 

C 

FTDIF ( I ) =FORCE ( I ) -FTBI AS ( I ) 

41 

RETURN 

42 

END 
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1  SUBROUTINE  GGNTRL  ( DATAFLG ,  S  VS  FLAG ,  RGB1 RAT ,  RQB1CMD  /  ROB?  DAT ,  ROB2CMD , 

l  FTSAMPl",  Ff S^MPg)  ' 

g 

g  .NAME?  gphtrpl 
c 

e  PURPpSE :  Perfopas  high  rate  (inner  iopp)  Fpppe/Tprgpe  §h$|  positioning 
G  calculations  to  interface  fh£  YAL  alter  cpmmap.g 

g 

g  DESCRIPTION?  h  rnv  set  of  For.ee/Tgp.gue  pgggihgsis  processed.  If 
G  £he  robots  have  started,  bjit  ere  i).ot  g|  fh®?-?'  U?iP  PP^hts, 

G  guidance  is  supplied  tp  the  new  grip  point.  After  the  grip  point 

C  is  rpachgd^  the  Fprpe/Torque  matrix'  multiplies  each  forpe  reading 

e  to  obtain  a  new  sgt  of  .displacements, 

e  ... 

C  INPUTS?  DATAFLG  -  Flags  showing  which  data  is  y§lid  on  input,  out 

c  SYSFLAG  -  Flags  showing  the  state  of  the  system 

C  ROB i DAT  -  input  jiata  frpm  rph.ot  1  igpigding  positional  tr 

G  ROB2DAf  -  input  data  froiij  robpt  2  including  ppsitional  tr 

C  FTS^MRi  -  Twp  Forcg/Torque  sgpples  from  the  wrist  of  robo 

C  FTS^MP2  -  Two  Force/Torgue  samples  frpm  the  wrist  of  robo 

0 

e  OUTPUTS;  ROBICMD  =•  New  positional  delta  for  robot  1 

C  R0B2CMB  -  New  positional  pelfa  for  robot  2 

C 

2  INTEGER* 2  ROBIDAT(IS) ,R0B2DAT(15)  , R0B1GND(7) ,ROB2CMp(7) , 

1  FTSAMP1  (36)  ,  FTSARP2  (36)  , JDFLAG 

3  INTEGER*!  DATAFLG, SYSFL^G, IDFLAG 

4  EQUIVALENCE  (IDFLAG, JDFLAG) 

C 

$IN.CLUDE  (FTORQUE ,  INU) 

=1  $NOLIST 

5=1  INTEGER  FTMFLAG ,  FTDpIT,  FTDONE ,  F.START ,  RTNFLAG2 ,  R0BDIF1 ,  R0BDIF2 

6=1  INTgGER  FTGRIP ,  R2MSG ,  GRIPPD ,  STARTD ,  R1MSG ,  F’PALG ,  AUTO ,  FTAUTO 

7=1  INTEGER  MVSKED , MVBAK , MVRAK 

8=1  pHARAGTER*4p  PROMPT 

9=1  CHARACTERS  BISPT 

10=1  LOGICAL*!  RESET ",  FTPLOT 

=1  G 

11=1  PARAMETER  (R2NSG=2,R1NSG=1) 

12=1  PARARgTER  (AUTQ=3  4 ,  GRIPPD=2  0 ,  STARTUP 65) 

13=1  PARAMETER  (FSTART=l/FTAUT0=2,FTGRiP=4,RTBPNE=§) 

i4=i  p^MgyER  (FfbQi^ie.NAysw^^fMoyEPPN^^fFc^Lc^i^s) 

15=1  PARAMETER  (NVSKED=l,MyBAK=2 ,MVRAK=4) 

=1  C 

16=1  COMMON/ FTCON/RORCE ( 14 )  ,  FTBIAS ( 14 ) ,FTDIF(14) , FSG^LE , TRCALE , TDZ 

17=1  G6MMON/FT§UM/FizSUN(6) , F2ZSUR(6) , FZSUlf  (6) ,FSUM(6) 

18=1  GOMMON/ETMRT/XG^lIN, YGAIN, ZGAIN, ALENTH, FTMAT2 (6,6) , 

=1  1  FTMATX (6,6), RSCALJ? ( 6 ) 

19=1  GOMMON/XFORM/eTTORl (3,3), GTT0W2 (3,3), GT00L2 (6,6) , 

=1  4  FTOOL1 ( 3 ) , FTOOL2 ( 3 ) , NTOOLi ( 3 ) , MTOOL2 ( 3 )  , GTOOL1  (3,3) 

20=1  GQNMON/VECT/D)j,  QY,PZ,  {)X2 ,  DY2 ,  PHIZ? ,  PRRIM 

21=1  COMMQN/MRGCOM/PROMET , RESET , FTPLOT (28 ) 

22=1  CONMON/MGNTL/IPUTgR, I FLAG, PISPT , FTMFLRp , FTMFLAG? 

23=1  CQNMdN/NAVEGT/BX ,  BY ,  BZ',  RACKPT  (4,2),  ROBPT1  ( 6 )  ,  ROBPT2  { 6 ) 

24=1  COMON/NAVEPT/R6BbiFl(6)  ,ROBDIF2(6)  ,ROBWDiFl(6)  ,ROBWDIF2(6)  , 

=i  1  RDMAX , RTMAX , NGALC 
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25 

26 

27 

28 


29 

30 


31 


32 

33 


34 

35 

36 

37 

38 

39 

40 

41 

42 


43 

44 

45 

46 

47 

48 

49 

50 

51 


C 

C 

C  Translate  force/torque  variables  to  floating  point 

c 

DO  10  1=6, 1,-1 

CALL  CKFTF ( I , FTSAMP1 ( 1+1 ) ) 

CALL  CKFTF ( 1+7, FTS AMP2 (1+1) ) 

10  CONTINUE 

C 

C 

IF  ( IAND  ( IFLAG ,  STARTD)  .  EQ .  STARTD)  THEN 
I DFLAG=DATAFLG 
C 

C  Translation  algorithms  to  new  rack  location 
C 

IF  (IAND (FTMFLAG, FTGRIP) .EQ.0)  THEN 
C 

C  If  both  robots  have  not  reached  the  grip  point,  then 
C  the  controller  sends  back  position  corrections  only 

C  derived  from  camera  input 

C 

IF (IAND (FTMFLAG, FCALC) .NE.0)  THEN 
NCALC=NCALC+1 
C 

C  Robot  Position  Commands 
C 

ROBlCMD(l) =256*63 
ROB2CMD(l) =256*63 
DO  20  1=1,6 

ROB1CMD ( 1+1 ) =R0BDIF1 ( I ) 

20  ROB2CMD(I+l)=ROBDIF2 (I) 

C 

JDFLAG=IOR ( JDFLAG, R2MSG) 

JDFLAG=IOR (JDFLAG , R1MSG) 

ENDIF 


C 

C  Force/torque  Control  Algorithms 
C 

C  After  both  robots  reach  grip  point,  both  navigational  and  force 
C  feedback  may  occur.  Coordinate  changes  due  to  camera  input  must 

C  be  removed  for  robot  to  reach  target  location.  After  navigation 

C  is  complete,  force  feedback  is  implemented. 

C 

ROB2CMD(l) =256*63 
ROBlCMD(l) =256*63 
IF ( IAND (FTMFLAG, FCALC) .NE.0)  THEN 
C  Do  navigational  movement 

NCALC=NCALC+1 
DO  30  1=1,6 

R0B1CMD ( 1+1 ) =R0BDIF1 ( I ) 

ROB2CMD(I+l) =R0BDIF2 (I) 

30  CONTINUE 

ELSE 

C  Do  force/torque  feedback 

DO  32  1=1,6 
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53 

54 

55 

56 

57 

58 

59 

60 
61 


62 

63 

64 

65 


67 

68 


XSUM=6iO 

IF  ( IAND ( FTMFiAG , FTbOIT) t  NE i 0 )  THEN 
DO  35  J=l,6 

35  ^SUM=XSUM+FTblF  ( J+"7 )  *FTMATX(I,J) 

ENDIF 

rObSOMS ( i+i ) =xsDM*r§@AlF ( i ) 
ROBiCMD(i+i)=o 
32  CONTINUE 

ENBlF 

c 

G  Set  robot  messages  are  available 
c 

JDFLAG=iOR ( JDFLAG , R1MSG) 

JDFLAG=I0R ( JDFLAG  >  R2MSG) 

eMdif 

bATAFLG=I DFLAG 
C 

ENDIF 

C 

RETURN 

END 
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9=1 

10=1 

11=1 

12=1 

=1 

13=1 
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15=1 

16=1 

17=1 

=1 
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20=1 

=1 

21=1 
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SUBROUTINE  OUTER (FORTFLAG , SYS FLAG, ROB1DAT , ROB1CMD, ROB2DAT , ROB2CMD 
1  , FTSAMP1 , FTSAMP2 ) 

C 

C  NAME;  Outer  Control  loops 
C 

C  PURPOSE:  Perform  low  rate  control  calculations  including  gravity 
C  compensation  and  navigation 

C 

C  DESCRIPTION:  This  routine  updates  system  flags  in  common  and  clears 
C  all  flags,  etc.  when  the  task  is  complete.  It  also  calls 

C  gravity  compensation  subroutine  and  navigation  subroutine  to 

C  update  (slow  moving)  quantities  for  the  next  group  of  iteration 

C 
C 

C  INPUTS : 

C 
C 
C 
C 
C 
C 

C  OUTPUTS : 

c 

C 
C 

INTEGER*2  R0B1DAT(15)  ,ROB2DAT(15) ,R0B1CMD(7) ,ROB2CMD(7) , 

1  FTSAMP1  (36),  FTSAMP2  (36),  JDFLAG ,  JFFLAG 

INTEGER  SEG , MOTION ,T11,T21,T31,T12,T22,T32,T13,T23,T33,T14, 

1  T24 ,T34 

PARAMETER ( SEG=2 ,  MOTION=3  ,  Tll=4 , T2 1=5 , T3 1=6 , T12=7 , T2  2=8 , 

1  T32=9 , T13=10,T23=11  ,T33=12 ,T14=13 , T24=14 , T34=15) 

INTEGER* 1  DATAFLG , SYSFLAG ,  IDFLAG , I FFLAG , FORTFLAG ( 2 ) 

EQUIVALENCE  (IDFLAG, JDFLAG) , (I FFLAG, JFFLAG) 

C 

$ INCLUDE  (FTORQUE. INC) 

$NOLIST 

INTEGER  FTMFLAG , FTDOIT ,  FTDONE, FSTART, FTMFLAG2 ,ROBDIFl,ROBDIF2 
INTEGER  FTGRIP , R2MSG , GRIPPD , STARTD , R1MSG, FCALC , AUTO , FTAUTO 
INTEGER  MVSKED , MVBAK , MVRAK 
CHARACTER* 40  PROMPT 
CHARACTER* 3  DISPT 
LOGICAL* 1  RESET, FTPLOT 
C 

PARAMETER  (R2MSG=2 , R1MSG=1) 

PARAMETER  ( AUTO=3 4 , GRIPPD=2  0 , STARTD=65) 

PARAMETER  (FSTART=1, FTAUTO=2 , FTGRIP=4 , FTDONE=8) 

PARAMETER  (FTD0IT=16,NAVSW=32 ,MOVEDONE=64,FCALC=128) 

PARAMETER  (MVSKED=1,MVBAK=2 ,MVRAK=4) 

C 

COMMON/FTCOM/FORCE ( 14 )  ,FTBIAS(14) ,FTDIF(14) , FSCALE , TSCALE , TDZ 
COMMON/FTSUM/F1ZSUM(6)  ,F2ZSUM(6)  ,FZSUM(6)  ,FSUM(6) 
COMMON/FTMAT/XGA.IN ,  YGAIN ,  ZGAIN,  ALENTH ,  FTMAT2  (6,6), 

1  FTMATX(6,6) ,RSCALE(6) 

COMMON/XFORM/ CTT0W1  (3,3),  CTTOW2  (3,3),  GTOOL2  (6,6), 

4  FTOOL1 ( 3 ) , FTOOL2 ( 3 )  ,  MTOOL1 ( 3 ) , MTOOL2 ( 3 ) , GTOOL1 (3,3) 
COMMON/VECT/DX, DY,  DZ ,  DX2 ,  DY2 , PHIZ 2 , DPHIM 


FORTFLAG  -  Flags  summarizing  the  system  state 

SYSFLAG  -  Flags  showing  the  state  of  each  robot  in  the  sy 

ROB1DAT  -  Input  data  from  robot  1  including  positional  tr 

ROB2DAT  -  Input  data  from  robot  2  including  positional  tr 

FTSAMP1  -  Two  Force/Torque  samples  from  the  wrist  of  robo 

FTSAMP2  -  Two  Force/Torque  samples  from  the  wrist  of  robo 


ROB1CMD  -  Robot  1  commands  zeroed  when  robot  1  is  done 
ROB2CMD  -  Robot  2  commands  zeroed  when  robot  2  is  done 
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23=1 

24=1 

25=1 

26=1 

=1 

C 

27 

C 

C 

C 

28 

29 

30 

31 

32 

33 

C 

C 

C 

34 

35 

36 

37 

38 

39 

40  10 

41 

42 

43 

44 

C 

C 

c 

45 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

46 

47 

C 

48 

C 

49 

50 

51 

52 

53 

54 


COMMON/MS  GCOM/ PROMPT , RESET, FTPLOT (28) 

COMMON/MCNTL/ 1  OUTER ,  IFLAG,  DISPT,  FTHFLAG,  FTMFLAG2 
GOMMON/NAVECT/BX, BY , BZ , RACKPT (4,2)  ,R0BPT1(6)  ,ROBPT2(6) 
COMMON/NAVEPT/ROBDIF1  ( 6 )  ,ROBDIF2(6)  ,R0BWDIF1(6)  ,R0BWDIF2(6)  , 

1  RDMAX , RTMAX , NC ALC 

IOUTER=IOIJTER+l 

Housekeeping  for  flags  and  variables 

JDFLAG=0 
JFFLAG=0 
IDFLAG=SYSFLAG 
IFFLAG=FORTFLAG ( 1 ) 

iflag=jdflag 

FTMFLAG=I AND ( FTMFLAG ,240)+IAND(JFFLAG,15) 

Reset  everything  when  robot  has  dropped  START 

IF (IAND (FTMFLAG, FSTART) .NE.FSTART)  THEN 
ROB1CMD(1)=0 
R032CMD(1)=0 
DO  10  1=1,6 

ROB1CMD ( 1+1) =0 
ROB2CMD(I+1)=0 
CONTINUE 
NCALC=0 
FTMFLAG=0 
FTMFLAG2=0 
ELSE 

Call  Gravity  Compensation  and  Make  Navigational  Calculations 
CALL  GRAV (R0B1DAT,R0B2DAT) 

Rack  coordinates  supplied  by  the  camera  are  represented  as  increments 
rack  coordinates  the  robot  is  taught.  To  properly  compensate, 
navigation  is  done  by  the  following  steps: 

1)  Move  to  grip  point  robot  was  taught 

2)  Compute  true  grip  point  from  camera  increments 

3)  Move  to  true  grip  point 

4)  Grasp  object 

5)  Compute  path  to  taught  grip  point 

6)  Move  back  to  taught  grip  point 

7)  Continue  force/torque  algorithms 

IF  ( IAND ( IFLAG, GRIPPD) .EQ.GRIPPD)  THEN 
IF  (IAND(FTMFLAG, FTGRIP) .EQ.0)  THEN 
At  grip  point,  but  not  gripped 

IF(IAND(FTMFLAG2,MVSRED) .NE.0)  THEN 
Then  compute  true  grip  point,  set  flags  to  do  only  once 
GALE  RAKM0V(1) 

FTMFLAG2=IAND(FTMFLAG2 , 255-MVSKED) 
FTMFLAG2=IOR(FTMFLAG2  ,MVBAK) 

FTMFLAG=IOR ( FTMFLAG , NAVSW) 

ENDIF 

ENDIF 
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55  ELSE 

C  Ob j ect  gripped 

56  IF  ( IAND  ( FTMFLAG2  ,  MVBAK)  .  NE .  0 )  THEN 

C  Compute  path  back  to  taught  grip  point,  reset  flags 

57  CALL  RAKMOV ( 2 ) 

58  FTMFLAG2=IAND ( FTMFLAG2 , 255-MVBAK) 

59  FTMFLAG=IOR  ( FTMFLAG ,  NAVSW) 

60  ENDIF 

61  ENDIF 
C 

C  Compute  movement  increments  for  current  path 

c 

62  CALL  NAV (ROB1DAT, ROB2DAT) 

63  ENDIF 
C 

C  Return  synchronization  flag  to  main  process 
C 

64  JFFLAG=IAND (FTMFLAG, 240) 

6  5  FORTFLAG ( 2 ) =1 FFLAG 

C 

66  RETURN 

67  END 
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1  SUBROUTINE  GRAV (ROBlDAT, ROB2DAT) 

C 

C 

C  NAME:  Gravity  Compensation 
C 

C  PURPOSES  Compute  external  forces  on  the  object  and  set  FTBIAS  to  refl 
C  these  external  forces 

C 

C  DESCRIPTION i  The  transformation  matrix  supplied  by  VAL  is  translated 
C  real  numbers.  A  vector  between  the  gflppers  is  also  calculated.  A 

C  dimensional  vector  representing  the  ditectioh  front  robot  2  to  robo 

C  in  robot  2  tool  coordinates  is  calcinated.  Forces  are  thexi  transl 

C  to  the  world  coordinate  system,  and  summed.  It  is  assumed  that  an 

C  forces  after  summation  is  due  to  external  forces,  and  these  forces 

C  load  shared  between  the  two  robots  by  setting  FTBIAS. 

c 

C  INPUTS:  ROBlDAT  -  Robot  1  transformation  data  supplied  by  VAL 
C  R0B2DAT  -  Robot  2  transfomration  data  supplied  by  VAL 

C 
C 

2  INTEGER* 2  ROBlDAT ( 15 ) ,ROB2 DAT (15) 

3  INTEGER  SEG, MOTION, Til, T21,T31,T12,T22,T32,T13,T23,T33,T14, 

1  T24  T34 

4  PARAMETER (SEG=2 ,  MOTIONS ,  Tll=4 ,  T2 1=5 ,  T3l=6 ,  T12=7 ,  T22=8 , 

1  T3  2=9, T13=10, T2 3=11 ,T3 3=12 , T14=13,T24=14 ,T34=15) 

C 

$ INCLUDE  (FTORQUE. INC) 

=1  $NOLIST 

5=1  INTEGER  FTMfLAG,FTDOIT,FTDONE,FSTART,FTMFLAG2,ROBDIF1,ROBDIF2 

6=1  INTEGER  FTGRiP , R2MSG , GRI PPD , STARTD , R1MSG ,  FCALC , AUTO , FTAUTO 

7=1  INTEGER  MVSKED, MVBAK, MVRAK 

8=1  CHARACTER* 40  PROMPT 

9=1  CHARACTER*3  DISPT 

10=1  LOGICAL* 1  RESET, FTFLOT 

=1  C 

11=1  PARAMETER  (R2MSG=2,R1MSG=1) 

12=1  PARAMETER  (AUTO=34 , GRIPPD=20 , STARTD=65) 

13=1  PARAMETER  ( FSTART=1 ,  FTAUT0=2  ,  FTGRIP=4 ,  FTDONE=8 ) 

14=1  PARAMETER  (FTD6lT=16,NAVSW=32,M0VED0NE=64,FCALC=128) 

15=1  PARAMETER  (MVsKED=1  ,  MVBAK=2  ,  MVRAK=4 ) 

=1  C 

16=1  COMMON/FTCOM/FORCE ( 14 ), FTBIAS (14) ,FTDIF(14) , FSCALE,TSCALE,TDZ 

17=1  00MM0N/FTSUM/F1ZSUM(6)  ,F2ZSUM(6)  ,FZSUM(6)  ,FSUM(6) 

18=1  COMMON/FTMAT/XGAIN, YGAIN,  ZGAIN,  ALENTH,  FTMAT2  (6 , 6)  , 

=1  1  FTMATX(6,6),RSCALE(6) 

19=1  C0MM0N/XF0RM/CTT0W1  (3,3),  CTTOW2  (3,3),  GT00L2  (6,6), 

=1  4  FT00L1  ( 3 )  ,  FI00L2  ( 3 )  ,  MT00L1  ( 3 )  ,  MT00L2  ( 3 )  ,  GT00L1  (3,3) 

20=1  COMMON/VECT/DX, DY, DZ , DX3 , DY2 , PHIZ 2 , DPHIM 

21=1  comMon/msgcoM/promrt,  Reset,  ftplot  (28) 

22=1  COMliON/MCNTL/idUTER, IFLAG , DISPT , FTMFLAG, FTMFLAG2 

23=1  COMMON/NAVECT/BX,BY,BZ,RACRPT(4,2) ,R0BPT1(6) ,ROBPT2(6) 

24=1  COMMON/NAVEPT/ROBDIFl ( 6 ) ,ROBDIF2 (6) ,R0BWDIF1 (6) ,ROBWDIF2 (6) , 

=1  1  RDMAX,RTMAX,NCALC 

C 

C  Calculate  transformations  and  position  Vectors  from  VAL  inputs 
C 
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Compute  Robot  positions 

RX1=REAL (ROB1DAT (T14 ) )/32.0 
RY 1=REAL (ROB1DAT (T2  4 ) ) /3  2 . 0 
RZ1=REAL(R0B1DAT(T34) )/32 . 0 
RX2=REAL (ROB2DAT (T14 ) ) /3  2 . 0 
RY2=REAL(ROB2DAT(T24) )/32.0 
RZ 2=REAL ( ROB2 DAT ( T3 4 ) ) / 3 2 . 0 

Compute  distances  between  grippers 

DX=BX+ (RX1-RX2 ) /25 . 4 
DY=BY+ (RY1-RY~ ) /25 . 4 
DZ=BZ+ (RZ1-RZ2) /25. 4 
TDZ=6.0 

Directional  cosines 

CTT0W1 (1/1) =REAL (R0B1DAT (4 ) )/16384.0 
CTTOW1 (2,1) =RE  AL ( ROB1DAT (5))/16384.0 
CTT0W1 (3,1) =REAL ( ROB1DAT (6))/16384.0 
CTTOW1 (1,2) =REAL (ROB1DAT (7 ) ) /1 63  84 . 0 
CTTOW1 (2,2) =REAL (ROB1DAT (8})/16384.0 
CTTOW1 (3,2) =REAL (ROB1DAT (9 ) ) /16384 . 0 
CTTOW1 (1,3) =REAL (ROB1DAT (10) )/16384.0 
CTT0W1(2,3)=REAL(R0B1DAT(11) )/16384.0 
CTTOW1 (3,3) =REAL (ROB1DAT ( 12 ) ) /16384 . 0 

CTTOW2 (1,1) =REAL ( ROB2  DAT(4))/16384.0 
CTTOW2 (2,1) =REAL ( ROB2  DAT (5))/16384.0 
CTTOW2 (3,1) =REAL ( ROB2  DAT (6))/16384.0 
CTTOW2 (1,2) =REAL (ROB2DAT (7) )/16384.0 
CTTOW2 (2,2) =REAL (ROB2DAT (8))/16384.0 
CTTOW2 (3,2) =RE  AL (ROB2  DAT (9))/16384.0 
CTTOW2 (1, 3) =REAL(ROB2DAT (10) )/16384.0 
CTTOW2 (2,3) =RE  AL ( ROB2  DAT (11) )/16384.0 
CTTOW2 (3,3)=REAL(ROB2DAT(12) )/16384.0 

Direction  to  Robot  1  in  Robot  2  Coordinates 

DX2=CTTOW2 (1,1) *DX+CTTOW2 (2,1) *DY+CTTOW2 (3 , 1) *DZ 
DY2=CTTOW2  (1,2)  *DX+CTTOW2  (2,2)  *DY-!-CTTOW2  (3,2)  *DZ 

Realign  the  control  Matrix 

CALL  T00LT2 (DX2 , DY2 , 0) 

Force/Torque  summation  calculations 


Translate  Forces  and  Torques  to  world  coordinates 

DO  5  1=1,3 
FTOOL1 (I) =0 . 0 
MTCOL1(I)=0, 0 
FTOOL2 (I) =0 . 0 
MTOOL2 (I)=0, 0 
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64 
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67 

68 

69 

70 
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72 

73 
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77 

78 

79 
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81 
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88 


DO  5  j=l,3 

FTOOL1  ( I )  =FTOOLl ( I )  +CTTOW1  (1,1)  *!cORCE (f-i) 

MT00L1  (I)=MTOOLl(i)  +CTT0W1  ( I ,  J)  *  FORCE  (4-j) 

FT00L2  (I)  =FT00L2  ( I )  +CTT0W2  ( I ,  J )  *  frORCE  ( 1 4  -  J) 

MT00L2  (I)  =MT00L2  (I) +CTT0W2  (I ,  J)  *  FORCE '(ll-J) 

5  CONTINUE 

C 

C  Sum  Forces  and  Torques  over  both  platforms 

C 

DO  15  1=1,3 
FIZSUM(I) =MTOOLl (4-1) 

F1ZSUM  ( 1+3 )  =FTt>OLl  (  4 -I ) 

F2 ZSUM ( I ) =MTOOL2 ( 4 -I ) 

F2 ZSUM (14-3)  =FTOOL2  ( 4-1 ) 

FZSUM ( I ) =F1ZSUM (I) -F2ZSUM ( I ) 

15  FZSUM ( 1+3 ) =F1ZSUM ( 1+3 ) +F2  ZSUM ( 1+3 ) 

C 

C  Moment  compensation  for  forces  at  the  other  platform 

C  (Weight  component  is  ignored  in  this  compensation) 

C 

C  Mz  =  Mz  +  Fx*Dy  +  Fy*Dx 

FZSUM (1) =FZSUM(1)+F1ZSUM(6) *DY-F1ZSUM(5) *DX 
C  My  =  My  +  Fx*Dz  +  Fz*Dx 

FZSUM ( 2 )  =FZSUN ( 2 )  -F1ZSUM ( 6 )  *DZ+ (FlZStjM(4)  -.5*FZSUM(4) )  *DX 
C  Mx  =  Mx  +  Fy*Dz  +  Fz*Dy 

FZSUM ( 3 ) =FZSUM ( 3 ) +F1ZSUM ( 5 ) *DZ- { F1ZSUM  ( 4 ) - . 5+FZSUM ( 4 ) ) *DY 
C 

C  Transform  the  Force  sum  to  Tool  coordinates  of  Robot  2 

c 

DO  25  1=1,3 
FSUM(I)=0.0 
FSUM(I+3) =0 . 0 
DO  25  J=l, 3 

FSUM ( I ) =FSUM ( I ) -FZSUM { J) *CTTOW2 ( 4- J , 4 -I ) 

FSUM ( 1+3 ) =FSUM ( 1+3 ) +FZSUM ( J+3 ) *CTT0W2 ( 4 - J , 4 -I ) 

25  CONTINUE 

C 

C  Calculate  load  balancing  biases 

C 

DO  20  1=1,6 

C  FTBIAS ( I ) =-FSUM ( I ) /2 . 0 

C  FTBIAS (1+7 )=FSUM (I) /2,0 

FTBIAS ( 1+7 ) = . 9  5*FTBIAS ( 1+7 )  + . 05*FSUM ( t ) /2 . 0 
20  CONTINUE 

C 

C  Did  we  hit  something  ? 

C 

C  (  CHECK  WHETHER  FZSUM  IS  POINTING  DOWN,  AND  HAS  CHANGED  SIGNIFlcAH 
C 

RETURN 

END 
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SUBROUTINE  NAV (ROB1DAT , ROB2 DAT) 

C 

C 

C  NAME:  Navigation 

c 

C  PURPOSE:  Compute  navigational  path  for  robot  to  reach  rack  point 
C 

C  DESCRIPTION:  If  the  robot  is  moving  toward  a  set  rackpoint,  this  rout 
C  calculates  the  incremental  rate  of  movement.  Movement  is  open  loo 

C  subroutine  CONTRL  counts  the  number  of  movements  it  has  commanded, 

C  this  subroutine  assumes  that  the  robot  has  actually  moved  that  num 

C  of  invrements  at  its  current  incremental  rate.  First,  the  routine 

C  updates  the  movement  goal  by  subtracting  the  distance  already  move 

C  Then,  it  calculates  the  movement  differential  by  dividing  the  move 

C  goal  by  25  (approx  no.  of  contrl  subr  executions/Nav  execution) . 

C  value  is  limited  to  prevent  severe  robot  motions.  Then,  these  mov 

C  are  transformed  to  tool  coordinates. 

C 

C  INPUTS:  ROB1DAT  -  Robot  1  transformation  data  supplied  by  VAL 
C  ROB2 DAT  -  Robot  2  transfomration  data  supplied  by  VAL 

C 
C 
C 

INTEGER* 2  R0B1DAT(15) ,ROB2DAT(15) 

INTEGER  SEG , MOTION , T1 1 , T2 1 , T3 1 , T12  ,  T2  2 , T3  2 , T13 , T2  3 , T3  3 , T14 , 

1  T24 ,T34 

PARAMETER ( SEG=2 , MOTION=3 , Tll=4 , T2 1=5 ,  T3 1=6 , T12=7 , T2  2=8 , 

1  T32=9 , T13=10,T2 3=11, T3 3=12  ,T14=13  ,T2 4=14 ,T34=15) 

C 

$ INCLUDE  (FTORQUE. INC) 

$NOLIST 

INTEGER  FTMFLAG ,  FTDOIT ,  FTDONE ,  FSTART ,  FTMFLAG2 ,  ROBDIF1 ,  ROBDIF2 
INTEGER  FTGRIP,R2MSG,GRIPPD,  STARTD , R1MSG ,  FCALC , AUTO ,  FTAUTO 
INTEGER  MVSKED , MVBAK , MVRAK 
CHARACTER* 40  PROMPT 
CHARACTER* 3  DISPT 
LOGICAL* 1  RESET, FTPLOT 
C 

PARAMETER  (R2MSG=2 , R1MSG=1) 

PARAMETER  (AUTO=34 , GRIPPD=20 , STARTD=65) 

PARAMETER  (FSTART=1 , FTAUTO=2 , FTGRIP=4 , FTDONE=8 ) 

PARAMETER  (FTDOIT=16 ,NAVSW=32 ,M0VED0NE=64  ,  FCALC=128) 

PARAMETER  (MVSKED=1 , MVBAK=2 , MVRAK=4 ) 

C 

COMMON/FTCOM/FORCE ( 14 ) , FTBIAS ( 14 )  ,FTDIF(14) , FSCALE , TSCALE , TDZ 
COMMON/ FTSUM/ FI 2 SUM ( 6 ) ,F2ZSUM(6)  ,FZSUM(6) ,FSUM(6) 
COMMON/FTMAT/XGAIN ,  YGAIN,  ZGAIN ,  ALENTH ,  FTMAT2  (6,6), 

1  FTMATX (6,6) , RSCALE ( 6 ) 

COMMON/ XFORM/ CTTOW 1 (3,3), CTTOW2 (3,3),  GTOOL2 (6,6), 

4  FTOOL1 ( 3 ) , FTOOL2  ( 3 ) ,MT00L1(3) ,MTOOL2(3)  ,GTOOLl(3,3) 
COMMON/VECT/DX,  DY,  DZ ,  DX2 ,  DY2 ,  PHIZ2 ,  DPHTM 
COMMON/MSGCOM/PROMPT ,  RESET ,  FTPLOT  (28) 

COMMON/MCNTL/ IOUTER ,  IFLAG,  DISPT,  FTMFLAG,  FTMFLAG2 
COMMON/NAVECT/BX,  BY ,  BZ  ,RACKPT(4 ,2)  ,ROBPTl(6)  ,ROBPT2(6) 
COMMON/NAVEPT/ROBDIF1  ( 6 )  ,R0BDIF2(6)  ,ROBWDIFl(6)  ,ROBWDIF2(6)  , 

1  RDMAX , RTMAX , NCALC 
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?§ 

26 

27 


28 

29 


30 


31 


32 

33 


34 

35 

36 

37 

38 

39 


40 

41 

42 

43 

44 

45 

46 

47 


48 

49 

50 

51 


52 

53 


If  (  I/^g  (  FT1JFL&G ,  NAySJ?)  .  NE .  Q )  THEjJ 
FTMFLAG=I AND ( fTMFLAG , 255rFCALC) 

|o§Moy?o.§ 

.G 

G  Update  f,or  Robgfc  1  pp§itipn  for  increments  traveled  so  far 

I  ■'* 

G  (ONLY  J$E1J  POINTS  ARE  I1JGRE$ENT£L) 

G  RQgPTl  ( 1)  sRQgPTl  (1)  -NGALCSfOpglFl  ( 1) 

GALL'  SUBZ  (ROBPTl(lj ", NGALG*Ro|^DIfl  ( 1) , ROBBT1  (1) ) 

g  mmi  1  j  hmm  < .? )  mwm  1 1 ) 

.GALE  SjNBZ  (RQBPT1  (2) N,GALG*RQ|WDIF1  (2)  , RGBPT1  (2) ) 

G  ROBPT1  (  6  j  =ROBPTl  ( 6 )  ‘=N.GALO*R@gflQIFl  ( ,6 j 

GALL  sy§Z(R.OBPTi(6)  ,3U§ALC*ROBWpiFi^)  ,ROBPT|(6) ) 

c 

G  Compute  Robot  .One  gesired  Differential  Mption  in  world  coordinates 

C  '  . ’ 

gOBWDIFl ( 1 ) -ROBET1 ( 1 )  /  2  5 . 0 
C  For  Absolute  points,’  usg 
G  ’  R6B^gIFl(l)='(RQBETl(l)rRXl)/25.G 

CALL  LIMZ (ROBWD1F1 (Ij , RDMAX) 

ROBWDIFf ( 2 ) =ROBPTl (2)/25.0 
G  For  Absolute  points,  use 
C  ’  ROBWDIF1  ( 2 )  ?  (ROBPT1  ( 2 )  .-RY1 )  /2  5 .  P 

mtii M  (loBWDiFl  (2)  ,  RDMAX) 

ROB5?DIfl(^?0.Q 

ROBjf  glFl  j  4  )  =50  •  Q 

ROBWDIFl ( 5 ) =6 . Q 

ROBWDiFi(G)=R6BPTl(6)/25.0 

GALL  LIMZ (ROBWDIFl (6) , RTMAX) 

P  ' 

C  Translate  to  tool  coordinates  for  Rgggt  l 
C 

DO  5  1=1,3 
ROBDIFX=0 . 0 
D0  6  J=l,  3 

R0BDIFX=R0BDIFX+CTT0W1 (I ,J) ^ROBWDIFl ( J) 

6  CONTINUE 

gpggIFl (I) =ROggIFX*RSGALE (I) 

ROBMO^ROBMOV+ROBDIFl (I) 

5  CONTltfUE 

C 

G  Need  angular  translation  here  for  gripping  angle 
C  ' 

D0  7  1=1,3 

ROBDIF1 ( 1+3 ) =RPBWDIF1 ( 1+3 ) *RSCALE ( 1+3 ) 

ROBMOV?RQBMOV+ROBDiFl ( 1+3 ) 

7  COlfTilfUE 
C 

C  Update  for  Robot  2  position  for  increments  traveled  so  far 

c 

C  (ONLY  WHEN  POINTS  ARE  IlfGRE$EIfT$L) 

C  ' R0BPT2 (1) =R0BPT2( 1 j rNGALG*ROBWPIF2 (1) 

CALL  SUBZ  (ROBPT2  (1)  ,lfGALG^RpB^gIF2  (lj  .ROBET2  (1) } 

C  ROBPT2'(2)=R6BPTn(2jrrjfGALC*RO§WDiF2(?j 

CALL  SUBZ  (ROBPT2  (2)  , NGALC*RQBif  DIF2  ( 2 )  ,RQBPT2  (2) ) 

C  ROBPT2  ( 6  j  r=ROBPT2  ( 6 )  rNCALC*RQBWDIF2  ( 6  j 
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54 


55 


56 

57 


58 

59 

60 
61 
62 
63 


64 

65 

66 

67 

68 

69 

70 

71 


72 

73 

74 

75 


76 

77 

78 

79 

80 
81 
82 

83 

84 

85 

86 


CALL  SUBZ (ROBPT2 (6) ,NCALC*ROBWDIF2 (6) ,ROBPT2 (6) ) 

C 

C  Compute  Robot  Two  Desired  Differential  Motion  in  world  coordinates 

c 

ROBWDIF2 ( 1 ) =R0BPT2 ( 1 ) / 2  5 . 0 
C  For  Absolute  points,  use 
C  ROBWDIF2 ( 1 )  =  (R0BPT2 ( 1 ) -RX2 ) /2  5 . 0 

CALL  LIMZ (R0BWDIF2 (1) ,RDMAX) 

R0BWDIF2 ( 2 ) =R0BPT2 ( 2 ) / 2  5 . 0 
C  For  Absolute  points,  use 
C  R0BWDIF2 ( 2 )  =  (R0BPT2 ( 2 ) -RY2 ) / 2  5 . 0 

CALL  LIMZ  (ROBWDIF2  (2)  ,RDMAX) 

ROBWDIF2 ( 3 ) =0 . 0 

ROBWDIF2 ( 4 ) =0 . 0 

ROBWDIF2 ( 5 ) =0 . 0 

ROBWDIF2 (6) =R0BPT2 ( 6 ) /25 . 0 

CALL  LIMZ (R0BWDIF2 ( 6 ) , RTMAX) 

C 

C  Translate  to  tool  coordinates  for  Robot  2 
C 

DO  9  1=1,3 
ROBDIFX=0 . 0 
DO  10  J=1 , 3 

ROBDIFX=ROBDIFX+CTTOW2 (I, J) *R0BWDIF2 (J) 

10  CONTINUE 

ROBDIF2 ( I ) =ROBDIFX*RSCALE ( I ) 

RO  BMO V=RO  BMO V +RO  BDI F2 (I) 

9  CONTINUE 

C 

C  Need  angular  translation  here  for  gripping  angle 
C 

DO  11  1=1,3 

ROBDIF2 (1+3) =R0BWDIF2 (1+3) *RSCALE(I+3) 

ROBMOV=ROBMOV+ROBDIF2 ( 1+3 ) 

11  CONTINUE 
C 

C  Update  count  variable  and  flag 
C 

NCALC=0 

FTMFLAG=IOR ( FTMFLAG , FCALC ) 

IF  (ROBMOV.EQ.O.O)  THEN 

FTMFLAG=IAND( FTMFLAG, 255-NAVSW) 

FTMFLAG=IOR ( FTMFLAG , MOVEDONE) 

ENDIF 

ELSE 

C  No  navigation 

FTMFLAG=IAND ( FTMFLAG , 2  5  5 -FCALC ) 

ENDIF 

C 

RETURN 

END 
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SUBROUTINE  RAKMbv(Mj 
C 

C  NAME:  Rack  Move 
c 

C  PURPOSE:  Compute  required  movement  of  rack  for  cobrdihdtes  received 
C 

C  description :  ThS,o££set  o£  thb  rdck  firom.  thb  ciirreht  coordinates  is 
C  computed,  ihbluding  the  required  rotatibriai  ahgib  at  eack  gripper. 

C 

C  INPUTS:  N  -  flag  . 

C  i  -  compute  fiove  to  rack  fr8m  current  position 

C  2  -  Compute  niovS  back  tb  pBsitibft  from  rack 

c 

$INCLUDE  (FTORQUE. INC) 

=i  $NOLIST 

=1  INTEGER  FT^FLAG, FTDOiT,FTDONE , FSTART^  i^TMFLAG2  ,ROBDIFl,ROBDIF2 

=1  INTEGER  f TGRIP ,  R2MS<3 ,  &RiPPD ,  STARTD ,  RiMSG ,  F&ALC  >  AUTO ,  FTAUTO 

=1  INTEGER  MVSKECi.MVBAk,MVRAk 

1  CHARACTER*  4  O'  PROMPT 

1  CHARACTER*  3  EilSPT 

1  LOGICAL*!  &ESET >  FTPLOT 

1  C  -  .  >  . 

1  PARAMETER  (R2MSG=2,R1MSG=1) 

1  PARAMETER  (AUTO=34iGRiPPD=2Ci,STARTb=6S) 

1  PARAMETER  ( FSTART=1 ,  FTAUTfcl*2 ;  FTGRIP=4 ;  FTDONE=8 ) 

1  PARAMETER  ( FTfci6lT=i6 , AVSW=!2 , k6VEb0NE=<54 ,  FCALC=i2 8 ) 

1  PARAMETER  (MVSKED=L,MVBAK=2 ,MVRAk=4) 

1C 

1  COMMON/FTCOM/FORCE (.14) , FTBIAS  (14)  ;  FTD±k  (14)  , FSCALE , TSCALE , TDZ 

1  COMMdN/FTSUM/Fl2SUM(6)  ,F2ZSUM(6)  >FZSUft(&)  ,FSUM(6) 

1  CCMMON/FTMAT/XGAIN  >  YGAIN ,  ZGAIN  >  ALENTH ;  FTMAT2  (6,6)  , 

1  1  FTMATX (6,6) , RSCALE ( 6 ) 

1  COMMON/ XpdRM/ CTTOW1  (3,3)'  > clTOWd  (3/3)  ^ GTOOL2  (6*6) , 

1  4  FTQOL1 (3 ) *  FTdOL2 ( 3 ) , MT00L1 ( 3  j , MT00L2 ( ! ) , GTOOL1  (3,3) 

1  fcOMMON/VkCT/DX, DY; DZ, DX2 , DY2 , PHIZ2 , DPHIM 

1  COMMON/MSGCdM/PROM^T ; &ESET, FTPLOT (28) 

1  CdMMQN/MCNTL/IOUTER ,  I  FLAG  ,  dlSPT ,  FMFLAg  j  FTMFLAG2 

1  COMMON/NAVECT/BX, §Y , BZ .  RACKPT (4,2). RdBPTl ( 6) , ROBPT2 ( 6 ) 

1  COM^ON/NAVEPT/ROBDiFl (6)  ,ROBDIF2(6)  ,R0BWDIF1  (8)  ,ROBWDIF2  (6) , 

1  1  RDMAX,RTMX;kcALC 


Compute  angle  to  move  robots 
IF  (N.EQ.lJ.  THfeN 

ANGLE=57 . 3 *  (ATAN2  (RACKPT  (1,1)4-25.4 *bx-RAbkPT (2,1)  > 

1  ,  .  RACKPT(1, 2)  4-25. 4*DY-RACKPT(2 ;  2) )  -ATAN2  (DX,DY) ) 

ENDIF 

Robot  Ohfe 

ROBPT1 ( 1 ) =rAckpt { ! , ! ) 

ROBPT1 ( 2 ) =kACKPT ( 1 , 2 ) 

RdBPTi(3)=d.o 
ROBPT1 ( 4 ) =0 . 6 
ROBPT1 (5) =0 . 0 
ROBPTl(6)=ANGLE 
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31 

32 

33 

34 

35 

36 


37 

38 

39 

40 

41 

42 

43 


C 

C  Robot  Two 
C 

R0BPT2 ( 1 ) =RACKPT (4,1) 

R0BPT2 ( 2 ) =RACKPT (4,2) 

R0BPT2 (3)=0. 0 
R0BPT2 (4) =0.0 
ROBPT2 (5) =0 . 0 
R0BPT2 ( 6 ) = ANGLE 
C 

C  To  move  back,  negate  all  values 

c 

IF(N.EQ. 2)  THEN 
DO  10  1=1,6 
ROBPT1 ( I ) =-R0BPTl ( I ) 

10  ROBPT2 ( I ) =-R0BPT2 ( I ) 

ENDIF 
C 

RETURN 

END 
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1 


2 

3 

4 

5 

6 


=1 

7=1 

8=1 

9=1 

10=1 

11=1 

12=1 

=1 

13=1 

14=1 

15=1 

16=1 

17=1 

=1 

18=1 

19=1 

20=1 

=1 

21=1 

=1 

22=1 

23=1 

24=1 


SUBROUTINE  PANEL ( SHFLAG , DATAFLG , FTSAMPL1 ,  FTSAMPL2 , SYSFLAG , TIME) 


a 

C  NAME:  Panel 

c 

C  PURPOSE:  Print  data  on  a  display  screen  fpr  monitoring 

c 

C  DESCRIPTION:  A  new  screen  of  data  is  formated  approximately  every  sev 
C  seconds.  The  top  of  the  screen  is  status  information  on  the  syst 

C  while  the  bottom  of  the  screen  can  be  changed  by  command  from  thi 

C  console.  The  available  screens  are: 

C  NAV  -  Display  navigation  data,  positions,  rates 

C  SUM  -  Display  force  summation  data,  base,  gripper  offsets 

C  XFR  -  Display  transformation  matrices  supplied  by  VAL 

C  MAT  -  Display  current  force/ feedback  matrix 

C  FOR  -  Display  raw  force/torque  readings 

C 
C 

C  INPUTS :  SHFLAG 
C  DATAFLG 

C  SYSFLAG 

C  robidat 

C  ROB2DAT 

C  FTSAMP1 

C  FTSAMP2 

C 


-  Shared  Common  Area  Flags 

-  Flags  showing  which  data  is  valid  on  input,  out 

-  Flags  showing  the  state  of  the  system 

-  input  data  from  robot  1  including  positional  tr 

-  Input  data  from  robot  2  including  positional  tr 

-  Two  Force/Torque  samples  from  the  wrist  of  robo 

-  Two  Force/Torque  samples  from  the  wrist  of  robo 


INTEGER* 1  SHFLAG (10) , DATAFLG, K, SYSFLAG 

INTEGER* 2  FTSAMPL1 (36), FTSAMPL2 ( 3  6 j , J , TIME ( 2 ) , IPC 

CHARACTER* 1  HEXDIG(16) 

DATA  HEXDIG/'O' , '1' , '2', '3', '4' ,'5', '6', '7', >8', '9',  'A', 
1  'B' , 'C* , 'D' , • E • , 'F'/ 

EQUIVALENCE  <J,K) 

C 

$INCLUDE  (FTORQUE.INC) 

$NOLIST 


INTEGER  FTNFLAG , FTDOIT ,  fTDONE , FSTART , FTMFLAG2 , ROBDIF1 , ROBDIF2 

INTEGER  FTGRIP,R2MSG,GRIPPD, STARTD,R1MSG, FCALC, AUTO, FTAUTO 

INTEGER  MVSKED,MVBAK,MVRAK 

CHARACTER* 40  PROMPT 

CHARACTER* 3  DISRT 

LOGICAL*!  RESET, FTPLOT 


PARAMETER  (R2MSG=2 ,R1MSG=1) 

PARAMETER  (AUTO=34 , GRIPPD=20 , STARTD=65) 

PARAMETER  ( FSTART=1, FTAUTO=2 , FTGRIP=4 , FTDONE=8 ) 
PARAMETER  (FTD0IT=I6 ,NAVSW=32 ,MOVEDONE=64 , FCALC=128) 
PARAMETER  (MVSKED=1 ,MVBAK=2 , MVRAK=4 ) 


COMMON/FTCOM/FORCE ( 14 ) , FTBIAS ( 14 ) , FTDIF ( 14 ) , FSCALE , TSCALE , TDZ 
COMMON/FTSUM/R1ZSUM  (6) ,F2ZSUM(6) ,FZSUM(6) ,FSUM(6) 
COMMON/FTMAT/XGAIN,  YGAIN,  ZGAIN,ALENTH,FTMAT2  (6, 6)  , 

1  FTMATX (6,6) , RSCALE ( 6 ) 

COMMON/ XFORM/ CTTOW 1 (3,3), CTT0W2 (3,3), GT00L2 (6,6), 

4  FT00L1 ( 3 ) , FfOOL2 ( 3 ) ,MT00L1(3) ,MTOOL2{3) ,GTOOLl(3,3) 
COMMON/VECT/DX, DY,  DZ , DX2 , DY2 , PHIZ2 , DPHIK 
COMMON/ MSGCOM/  PROMPT ,  RESET ,  FTPLOT  (28) 

COMMON/MCNTL/  IOUTER ,  I  FLAG,  DISPT,  FTMFLAG,  FTMFLAG2 
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25=1 

COMMON/NAVECT/BX ,BY,BZ, RACKPT (4,2), 

26=1 

COMMON/NAVEPT/ROBDIF1 (6) ,ROBDIF2 (6) 

=1 

1  RDMAX , RTMAX , NCALC 

c 

c 

Erase  the  screen,  new  page 

27 

c 

CALL  DISPCH ( CHAR (27)) 

28 

CALL  DISPCH (CHAR (42)) 

c 

c 

r\ 

Buffer  and  Flag  status 

29 

C. 

J=0 

30 

CALL  DISPCH ( 1  SHARE  BUFFER  FLAG  - 

31 

K=SHFLAG(1) 

32 

I=J/16 

33 

CALL  DISPCH (HEXDIG ( 1+1) ) 

34 

I=J-16*I 

35 

CALL  DISPCH(HEXDIG(I+1) ) 

') 

36 

CALL  DISPCH ('  FTMFLAG  - 

37 

K=FTMFLAG 

38 

I=J/16 

39 

CALL  DISPCH (HEXDIG (1+1) ) 

40 

I=J— 16*1 

41 

CALL  DISPCH(HEXDIG(I+1)  ) 

42 

CALL  DISPCH ('  FTMFLAG2  - 

') 

43 

K=FTMFLAG2 

44 

I=J/16 

45 

CALL  DISPCH (HEXDIG (1+1) ) 

46 

I=J-16*I 

47 

CALL  DISPCH(HEXDIG(I+1)  ) 

48 

CALL  DISPCH (CHAR (13)) 

49 

CALL  DISPCH (CHAR(IO)) 

50 

CALL  DISPCH ( CHAR ( 10 )  ) 

c 

rx 

Device  status 

51 

CALL  DISPCH (' DEVICE  FLAGS  - 

') 

52 

I=SHFLAG(2) 

53 

CALL  DISPCH ('ROB  1:  ») 

54 

IF  ( IAND ( SHFLAG ( 2 ) , 1 ) . NE . 0 ) 

THEN 

55 

CALL  DISPCH ('ON  ') 

56 

ELSE 

57 

CALL  DISPCH ('  ') 

58 

ENDIF 

59 

CALL  DISPCH ('ROB  2:  ') 

60 

IF  (IAND (SHFLAG (2) ,2) .NE.O) 
CALL  DISPCH ('ON  ’) 

THEN 

61 

62 

ELSE 

63 

CALL  DISPCH ('  ') 

64 

ENDIF 

65 

CALL  DISPCH ('F/T  1:  ') 

66 

IF  (IAND (SHFLAG (2) ,4) .NE.O) 

THEN 

67 

CALL  DISPCH ('ON  ') 

68 

ELSE 

69 

CALL  DISPCH ('  ') 

70 

ENDIF 

71 

CALL  DISPCH ('F/T  2:  ') 

ROBPT1 (  6 )  , ROBPT2 ( 6 ) 
,ROBWDIFl(6) ,ROBWDIF2(6) , 


') 
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IF  (IAND(SHFLAG(2) ,8) .NE.O)  1 
CALL  DISPCH ( 'ON  ') 

THEN 

73 

74 

ELSE 

75 

CALL  DISPCH («  ») 

76 

ENDIF 

77 

CALL  DISPCH (’BRD  Is  ») 

78 

IF  (IAND(SHFLAG(2) ,16) .NE.O) 

THEN 

79 

CALL  DISPCH ('ON  ') 

80 

ELSE 

81 

CALL  DISPCH ('  ') 

82 

ENDIF 

83 

CALL  DISPCH ('BRD  2i  ') 

84 

IF  (IAND(SHFLAG(2) ,32) .NE.O) 

THEN 

85 

CALL  DISPCH ('ON  ') 

86 

ELSE 

87 

CALL  DISPCH ('  ') 

88 

ENDIF 

89 

CALL  DISPCH ( CHAR ( 13 ) ) 

90 

CALL  DISPCH (CHAR (10)) 

91 

IF  (IAND(SHFLAG(2) ,64) .NE.  0) 
CALL  DISPCH (' 

THEN 

92 

93 

CALL  DISPCH ( 'ALGORITHM  ACTIVE') 

94 

CALL  DISPCH (CHAR (13 ) ) 

95 

ENDIF 

96 

CALL  DISPCH (CHAR (10)) 

97 

p 

CALL  DISPCH (CHAR (10)) 

V -r 

c 

p 

Error  Flags 

98 

V- 

CALL  DISPCH ( 'ERROR  FLAG  ROBOT  1  - 

99 

K=SHFLAG ( 3 ) 

100 

I=J/16 

101 

CALL  DISPCH (HEXDIG(I+1)) 

102 

I=J-16*I 

103 

CALL  DISPCH (HEXDIG ( 1+1) ) 

104 

CALL  DISPCH ('  ') 

105 

K-SHFLAG ( 5 ) 

106 

I=J/16 

107 

CALL  DISPCH (HEXDIG (1+1) ) 

108 

I«J-16*I 

109 

CALL  DISPCH (HEXDIG (1+1) ) 

110 

CALL  DISPCH ('  NAV 

LOOPS 

111 

CALL  DISPNO (NCALC,  0) 

112 

CALL  DISPCH (CHAR (13 ) ) 

113 

CALL  DISPCH (CHAR (10) ) 

114 

p 

CALL  DISPCH ( CHAR ( 10 ) ) 

115 

CALL  DISPCH ('ERROR  FLAG  ROBOT  2  - 

116 

K=SHFLAG(4) 

117 

I=J/16 

118 

CALL  DISPCH (HEXDIG (1+1) ) 

119 

I=J-16*I 

120 

CALL  DISPCH (HEXDIG (1+1) ) 

121 

CALL  DISPCH ('  ') 

122 

K=SHFLAG ( 6 ) 

123 

I=J/16 

124 

CALL  DISPCH (HEXDIG (1+1) ) 
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125 

I=J-16*I 

126 

CALL  DISPCH (HEXDIG ( 1+1 ) ) 

127 

CALL  DISPCH ('  OUTER  LOOPS  = 

') 

128 

CALL  DISPNO ( IOUTER , 0 ) 

129 

IOUTER=0 

130 

CALL  DISPCH (CHAR (13)) 

131 

CALL  DISPCH ( CHAR ( 10 ) ) 

132 

n 

CALL  DISPCH (CHAR (10)) 

133 

CALL  DISPCH ( ’OVERFLOW  FLAG  BOARD  1  - 

•) 

134 

K=SHFLAG ( 7 ) 

135 

I=J/16 

136 

CALL  DISPCH (HEXDIG (1+1) ) 

137 

I=J-16*I 

138 

CALL  DISPCH (HEXDIG (1+1)) 

CALL  DISPCH ('  ’) 

139 

140 

K=SHFLAG(9) 

141 

I=J/16 

142 

CALL  DISPCH (HEXDIG (1+1) ) 

143 

I=J-16*I 

144 

CALL  DISPCH (HEXDIG ( 1+1) ) 

145 

CALL  DISPCH (CHAR ( 13 ) ) 

146 

CALL  DISPCH ( CHAR ( 10 ) ) 

147 

r • 

CALL  DISPCH ( CHAR ( 10 ) ) 

148 

CALL  DISPCH ( ’OVERFLOW  FLAG  BOARD  2  - 

') 

149 

K=SHFLAG ( 8 ) 

150 

I=J/16 

151 

CALL  DISPCH (HEXDIG (1+1)) 

152 

I=J-16*I 

153 

CALL  DISPCH (HEXDIG (1+1) ) 

CALL  DISPCH (’  ') 

154 

155 

K=SHFLAG(10) 

156 

I=J/16 

157 

CALL  DISPCH (HEXDIG (1+1) ) 

158 

I=J-16*I 

159 

CALL  DISPCH (HEXDIG (1+1) ) 

160 

CALL  DISPCH (CHAR ( 13 ) ) 

161 

CALL  DISPCH (CHAR (10) ) 

162 

n 

CALL  DISPCH ( CHAR ( 10 ) ) 

c 

p 

Branch  here  to  individual  display  screens 

163 

IF(DISPT.EQ. 'NAV')  GOTO  3010 

164 

IF(DISPT.EQ. 'SUM')  GOTO  3030 

165 

IF(DISPT.EQ. ’XFR')  GOTO  3050 

166 

IF(DISPT.EQ. 'MAT')  GOTO  3060 

c 

p 

FOR  -  Display  raw  force/torque  readings 

167 

CALL  DISPCH ('  FORCE/TORQUE  SENSOR  1 

168 

CALL  DISPNO (FTSAMPL1 ( 1) ,0) 

169 

CALL  DISPCH (CHAR ( 13 ) ) 

170 

CALL  DISPCH ( CHAR ( 10 ) ) 

171 

CALL  DISPCH (CHAR ( 10 ) ) 

172 

DO  10  1=1,7 

173 

CALL  DISPNO (FTSAMPL1 (1+1) ,0) 
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174 

175  10 

176 
111 
178 


CALL  DISPCH ( 1  ') 
CONTINUE 

CALL  DISPCH (CHAR (13)) 
CALL  DISPCH (CHAR (10)) 
CALL  DISPCH (CHAR (10)) 


179 

180 
181 
182 

183 

184 

185 

186 

187  20 

188 

189 

190 

191 


CALL  DISPCH ('  FORCE/TORQUE  SENSOR  2 
CALL  DiSptf6(FTSAMPL2(l) ,6) 

CALL  DISPeH(GHAR(13) ) 

CALL  DISPCH (CHAR ( 10 ) ) 

CALL  DISPCH ( CHAR ( 10 ) ) 

DO  20  1=1,7 

CALL  DISPNO (FTSAMPL2 (1+1) , 0) 

CALL  DISPCH (*  ') 

CONTINUE 

CALL  DISPCH (CHAR ( 13 ) ) 

CALL  DISPCH ( CHAR ( 10 ) ) 

CALL  DISPCH (CHAR (10)) 

GO  TO  5000 


192 

193 

194 

195 

196 

197 

198 

199 

200 
201 
202 

203 

204 

205 

206 

207 

208 

209 

210 
211 
212 

213 

214 

215 

216 

217 

218 

219 

220 
221 
222 

223 

224 

225 

226 


Q 

C  NAV  -  Display  Navigational  data 

3010  CALL  DISPCH  (*  ROBOT  1  ALTER  -  RACK 
I=10*RACKPT (1,1) 

CALL  DISPNO (I, 2) 

CALL  DISPCH ('  Yl=‘) 

I=10*RACKPT(1,2) 

CALL  DISPNO (I, 2) 

CALL  DISPCH ('  X2=') 

I=10*RACKPT(2 , 1) 

CALL  DISPNO (I, 2) 

CALL  DISPCH ('  Y2=* ) 

I=10*RACKPT(2,2) 

CALL  DISPNO (I, 2) 

CALL  DISPCH (CHAR ( 13 ) ) 

CALL  DISPCH ( CHAR ( 10) ) 

DO  11  1=1,6 
J=10*ROBPT1 (I) 

CALL  DISPNO (J, 2) 

CALL  DISPCH (»  ') 

11  CONTINUE 

CALL  DISPCH (CHAR (13)) 

CALL  DISPCH ( CHAR ( 10 ) ) 

DO  12  1=1,6 
J=10*ROBWDIF1(I) 

CALL  DISPNO (J, 2) 

CALL  DISPCH ('  ’) 

12  CONTINUE 

CALL  DISPCH (CHAR ( 13 ) ) 

CALL  DISPCH (CHAR (10)) 

DO  13  1=1,6 

J=10* (R0BDIF1 (I)/RSCALE (I) ) 

CALL  DISPNO (J, 2) 

CALL  DISPCH ('  ’) 

13  CONTINUE 

CALL  DISPCH (CHAR (13)) 

CALL  DISPCH (CHAR (10)) 
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227 

228 

229 

230 

231 

232 

233 

234 

235 

236 

237 

238 

239 

240 

241 

242 

243 

244 

245 

246 

247 

248 

249 

250 

251 

252 

253 

254 

255 

256 

257 

258 

259 

260 
261 

262 


263 

264 

265 

266 

267 

268 

269 

270 

271 

272 

273 

274 

275 

276 

277 

278 


CALL  DISPCH( 1  ROBOT  2  ALTER  -  RACK 
I=10*RACKPT(3,1) 

CALL  DISPNO(I,2) 

CALL  DISPCH ( 1  Yl=») 

I=10*RACKPT (3,2) 

CALL  DISPNO (I , 2) 

CALL  DISPCH ( '  X2=») 

1=10  *RACKPT (4,1) 

CALL  DISPNO (I, 2) 

CALL  DISPCH (•  Y2= 1 ) 

I=10*RACKPT(4,2) 

CALL  DISPNO (I ,2) 

CALL  DISPCH (CHAR (13)) 

CALL  DISPCH (CHAR (10) ) 

DO  14  1=1,6 
J=10*ROBPT2 (I) 

CALL  DISPNO (J, 2) 

CALL  DISPCH ('  ') 

14  CONTINUE 

CALL  DISPCH (CHAR (13 ) ) 

CALL  DISPCH (CHAR (10) ) 

DO  15  1=1,6 
J=10*ROBWDIF2 (I) 

CALL  DISPNO(J, 2) 

CALL  DISPCH ('  ') 

15  CONTINUE 

CALL  DISPCH (CHAR (13)  ) 

CALL  DISPCH (CHAR (10)) 

DO  16  1=1,6 

J=10* (ROBDIF2 (I) /RSCALE (I) ) 

CALL  DISPNO (J, 2) 

CALL  DISPCH ( 1  «) 

16  CONTINUE 

CALL  DISPCH (CHAR (13) ) 

CALL  DISPCH ( CHAR ( 10 ) ) 

c 

GO  TO  5000 
C 

c  SUM  -  Display  force/torque  summation  data 

3030  CALL  DISPCH('  FORCE  Z  1  ' ) 

DO  21  1=1,6 
J=10*F1ZSUM(I) 

CALL  DISPNO (J, 2) 

CALL  DISPCH (‘  >) 

21  CONTINUE 

CALL  DISPCH (CHAR (13) ) 

CALL  DISPCH (CHAR ( 10) ) 

CALL  DISPCH ( '  FORCE  Z  2  « ) 

DO  22  1=1,6 
J=10*F2ZSUM(I) 

CALL  DISPNO (J, 2) 

CALL  DISPCH ('  ') 

22  CONTINUE 

CALL  DISPCH (CHAR (13) ) 

CALL  DISPCH (CHAR (10) ) 


Xl=* 
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279 

CALL  DISPCH ( '  DOWN  FORCE  *) 

280 

DO  23  1=1,6 

281 

J=10*FZSUM(I) 

282 

CALL  DISPNO(J,2) 

283 

CALL  DISPCH (•  «) 

284 

23 

CONTINUE 

285 

CALL  DISPCH (CHAR (13)) 

286 

CALL  DiSPGH(CHAi?(10)) 

287 

C£LL  DISPCH ('  FORCE  DIF  ») 

288 

DO  24  1=4,6 

289 

J=10*FTPIF(I+7) 

290 

CALL  DISPNO ( J, 2 ) 

291 

CALL  DISPCH (*  ') 

292 

24 

CONTINUE 

293 

CALL  DISPCH (CHAR (13)) 

294 

CALL  DISPCH (CHAR (10)) 

295 

CALL  DISPCH (CHAR (10) ) 

296 

CALL  DISPCH ( '  BAR  SIZE  ») 

297 

J=10*DX 

298 

CALL  DISPNO (J, 2) 

CALL  DISPCH ('  ') 

299 

300 

J=10*DY 

301 

CALL  DISPNO (J, 2) 

302 

CALL  DISPCH ('  ') 

303 

J=10*DZ 

304 

CALL  DISPNO (J, 2) 

305 

CALL  DISPCH ('  ') 

306 

CALL  DISPCH (CHAR (13 ) ) 

307 

CALL  DISPCH (CHAR ( 10 ) ) 

308 

CALL  DISPCH ('  BASE  DIF  ») 

309 

J=10*BX 

310 

CALL  DISPNO (J, 2) 

311 

CALL  DISPCH ('  ') 

312 

J=10*BY 

313 

CALL  DISPNO (J, 2) 

314 

CALL  DISPCH ('  ') 

315 

J=10*BZ 

316 

CALL  DISPNO (J, 2) 

317 

CALL  DISPCH ('  «) 

318 

CALL  DISPCH (CHAR ( 13 ) ) 

319 

CALL  DISPCH (CHAR (10) ) 

320 

CALL  DISPCH (CHAR (10) ) 

321 

n 

GOTO  5000 

c 

n 

XFR  -  Display  current  robot  transformation 

322 

V* 

3050 

CALL  DISPCH ('  ROBOT 

1 

TRANSFORM 

323 

CALL  DISPCH ( 1  ROBOT 

2 

TRANSFROM 

324 

CALL  DISPCH (CHAR ( 13 ) ) 

325 

CALL  DISPCH ( CHAR ( 10 ) ) 

326 

CALL  DISPCH (CHAR ( 10 ) ) 

327 

DO  34  L=i , 3 

328 

DO  32  1=1,3 

J=1000*CTTOW1 (L, I) 

329 

330 

CALL  DISPNO (J, 4) 

331 

CALL  DISPCH ('  ') 

332 

32 

CONTINUE 

cn 
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333 

CALL  DISPCH ( *  ') 

334 

DO  33  1=1,3 

335 

J=1000*CTTOW2 (L, I) 

336 

CALL  DISPNO(J,4) 

337 

CALL  DISPCH (•  ') 

338 

33 

CONTINUE 

339 

CALL  DISPCH  (CHAP.  (13)) 

340 

CALL  DISPCH (CHAR (10 ) ) 

341 

CALL  DISPCH ( CHAR ( 10 ) ) 

342 

34 

CONTINUE 

343 

GOTO  5000 

C  ] 

r\ 

MAT  -  Display  force  feedback  matrix 

344 

L- 

3060 

CALL  DISPCH (*  PHIZ  -  ‘ ) 

345 

J  =  100*PHIZ2 

346 

CALL  DISPNO(J, 3) 

347 

CALL  DISPCH ('  DX2  -  ‘ ) 

348 

J  =  100*0X2 

349 

CALL  DISPNO ( J, 3) 

350 

CALL  DISPCH ( 1  DY2  -  ') 

351 

CALL  DISPCH ( '  ') 

352 

J  =  100*DY2 

353 

CALL  DISPNO (J, 3) 

354 

CALL  DISPCH ( CHAR ( 13 ) ) 

355 

CALL  DISPCH (CHAR ( 10 ) ) 

356 

DO  38  L=l, 6 

357 

DO  36  1=1,6 

358 

J=100000*FTMATX (L, I) 

359 

CALL  DISPNO (J, 6) 

360 

CALL  DISPCH ('  ') 

361 

36 

CONTINUE 

362 

CALL  DISPCH (CHAR ( 13 ) ) 

363 

CALL  DxSPCH(CHAR(10) ) 

364 

38 

CONTINUE 

C 

C  Complete  display,  print  prompt 


365 

u 

5000 

J=0 

366 

CALL  DISPCH ( ‘SYSTEM  FLAG 

- 

367 

K=SYSFLAG 

368 

I=J/16 

369 

CALL  DISPCH (HEXDIG (1+1)  ) 

370 

I=J-16*I 

371 

CALL  DISPCH(HEXDIG(I+1)  ) 

CALL  DISPCH ( *  •) 

372 

373 

CALL  DISPCH ('IDLE  TIME  -  ') 

374 

FPC=100 . 0*TIME ( 1) /TIME ( 2 ) 

375 

IPC=FPC 

376 

CALL  DISPNO (IPC,0) 

377 

c 

CALL  DISPCH ( '  PERCENT  ') 

373 

CALL  DISPCH (CHAR ( 13 ) ) 

379 

CALL  DISPCH (CHAR ( 10) ) 

380 

c 

CALL  DISPCH ( CHAR ( 10 ) ) 

381 

CALL  DISPCH (PROMPT) 
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G 

6  ii  f§se€  iiig  set>  feset  §rr6r  fiags 

C 

382  iP  (RESET)  THEN 

383  5MfM6(SJ=6 

384  §HRL&3(6)=6 

385  sMFMg(§)sd 

386  SHFM§(16)=6 

387  ENMf 

388  RESET- . #ALSE . 

389  RETURN 

390  END 
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1  SUBROUTINE  DACDO (FTSAMP1 , FTSAMP2 ) 

C 

C  NAME:  DAC  Routime 
C 

C  PURPOSE:  Output  data  to  DAC  for  plotting 

c 

C  DESCRIPTION:  Calls  assembler  DAC  output  routine  seven  times 
C 

C  INPUTS:  FTSAMP1  -  Seven  force/torque  readings  from  robot  1 
C  FTSAMP2  -  Seven  force/ torque  readings  from  robot  2 

C 

2  INTEGER*2  FTSAMP1 (7) , FTSAMP2 (7) , I 
C 

DO  10  1=1,7 
CALL  DAC ( FTSAMP2 ( I ) , I ) 

10  CONTINUE 
C 

RETURN 
END 
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1  SUBROUTINE  GoMMb ( LINE , NSW) 

2  CHARACTER* (*)  LINE 
C 

G  NAME :  Command 
G 

G  PURPOSE:  Process  input  tommands  and  take  proper  action 
G 

C  DESCRIPTION: 

C 

C  INPUTS:  LINE  -  Command  Line 

G  NSW  -  Switch  for  communicating  with  assembler  driver 

C 

c 

3  CHARACTER* 1  CMOS (10) , NOS (6) , CHARA, DIRS (5) 

4  DIMENSION  DEL (6) 

C 

$ INCLUDE  ( FTORQUE . INC ) 

=1  $NOLIST 

5=1  INTEGER  FTMFLAG , FTDOIT  , FTDONE , FSTART  j  FTMFLAG2 , R6BDIF1 , ROBDIF2 

6=1  INTEGER  FTGRIP,R2MSG,GRIPPD,STARTD,RlMSG,FeALC,AUTO,FTAUTO 

7=1  INTEGER  MVSKED/MVBAK,MVRAK 

8=1  CHARACTER*  4  6  PROMPT 

9=1  CHARACTER* 3  DISPT 

10=1  LOGICAL*!  RESET, FTPLOT 

=1  C 

11=1  PARAMETER  (R2MSG=2 ,R1MSG=1) 

12=1  PARAMETER  (AUTO=34 ,GRIPPD=20 , STARTb=65) 

13=1  PARAMETER  (FSTART=1 , FT AUTO =2 , FTGRIP=4 , FTDONE=8 ) 

14=1  PARAMETER  (FTbOIT=16 ,NAVSW=32 ,MOVED0NE=64 , FCALC=l28) 

15=1  PARAMETER  (MVSKED=1,MV8AK=2,MVRAK=4) 

=1  C 

16=1  COMMON/ FTCOM/ FORCE ( 14 ) , FTBlAS ( 14 ) , FTDIF ( 14 ) , FSCALE , TSCALE , TDZ 

17=1  GOMMON/FTSUM/FlZSUM(6)  ,F2ZSUM(6)  ,FZSUM(6)  ^FSUMfe) 

18=1  COMMON/FTMAT/XGAlN,YGAIN,ZGAIN,ALENTH,FTMAT2(6,6) , 

=1  1  FTMATX (6,6) , RSCALE ( 6 ) 

19=1  COMMoN/XFORM/CTTOWI (3,3), CTT0W2 (3,3), GTOOL2 ( 6  f 6 ) , 

=1  4  FTOOL1 ( 3 ) j  FTOOL2 ( 3 ) , MTOOL1 ( 3 ) , MTOOL2 ( 3 ) , GTOOL1 (3,3) 

20=1  COMMON/ VEGT/feX  f DY , DZ , DX2 , DY2 , PHIZ2 , DPHiM 

21=1  COMMON/MSGGOM/PROMPT , RESET , FTPLOT (28) 

22=1  COMMON/MgNTL/IGUTER, IFLAG, Dl6PT, FTMFLAG,  FTMFLAG2 

23=1  GOMMON/NAVECT/BX,BY,BZ,RACKPT(4,2) ,ROBPTl(6) ,Rd8PT2(6) 

24=1  COMMON/NAVEPT/ROBDIFl  (6)  ,R0BDIF2(6)  ,ROBWi5±Fl  (6)  ,ROBWDIF2(6)  , 

=1  1  RDMAX , RTMAX , NCALC 

C 

25  DATA  CMDS/'R' , *M' , » P • , 'S' , 'G» , 'K» , * D • , 'C' , 'Q* , 'F'/ 

26  DATA  NOS/ '1', *2', '3' , '4' , '5', '6'/ 

27  DATA  DEL/10.0,10.0,10.0,10.0,10.0,10.0/ 

28  DATA  DIRS/  ’X',  '  Y  * ,  *  12  4 ,  'A',  *  P  •  / 

C 

29  IF  (NSW.EQ.2)  GOTO  100 
C 

30  DO  1  1=1,10 

31  IF  (LINE (1:1) .EQ.CMDS(i) )  GOTO  5 

32  1  CONTINUE 

33  PROMPT= ' COMMAND  NOT  KNOWN' 

34  RETURN 
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35 

5 

PROMPT=' ENTER  COMMAND* 

36 

p 

GOTO  (10,20,30,40,50,60,70,80,90,99) ,1 

c 

rt 

R  -  Reset  command 

37 

10 

RESET= . TRUE . 

38 

NSW=1 

39 

FTMFLAG=I AND ( FTMFLAG ,255 -NAVSW-FTDOIT-MOVEDONE-FCALC ) 

40 

DO  15  11=1,6 

41 

ROBDIF1 (11) =0 

42 

ROBDIF2 (11) =0 

43 

ROBWDIF1 (11) =0 . 0 

44 

ROBWDIF2 (11) =0 . 0 

45 

ROBPT1 (11) =0 . 0 

46 

15 

ROBPT2 (11) =0 . 0 

47 

p 

RETURN 

C 

p 

M  -  Modify  command 

48 

V— 

20 

CHARA=LINE (3:3) 

49 

DO  21  11=1,5 

50 

IF ( CHARA . EQ . DIRS (11) )  GOTO  24 

51 

21 

CONTINUE 

52 

PROMPT= ' BAD  MODIFY  DIRECTION* 

53 

RETURN 

54 

24 

1=5 

55 

CALL  GETNO ( LINE , I , FNO , CHARA) 

56 

GO  TO  (22,23,25,27,28) ,11 

57 

RETURN 

58 

22 

XGAIN  =  FNO 

59 

GOTO  26 

60 

23 

YGAIN  =  FNO 

61 

GOTO  26 

62 

25 

ZGAIN  =  FNO 

63 

GOTO  26 

64 

27 

ALENTH=FNO 

65 

26 

CALL  GAINM 

66 

CALL  TOOLT2 (0.0, 0.0,1) 

67 

RETURN 

68 

28 

DPHIM=FNO 

69 

P 

RETURN 

C 

c 

P  -  Print  command  -  no  longer  implemented 

70 

V* 

30 

p 

RETURN 

c 

p 

S  -  Enter  New  Co-ordinates  for  robotic  movement 

71 

40 

1=3 

72 

DO  42  11=1,4 

73 

CALL  GETNO ( LINE , I , RACKPT (11,1), CHARA) 

74 

IF( (CHARA. NE. * , ' )  . OR. (I . LT. 0) )  GOTO  43 

75 

CALL  GETNO (LINE, I, RACKPT (I 1,2) , CHARA) 

76 

IF(  (I1.EQ.4) .AND. (I.LT.0) )  GOTO  45 

77 

IF( (CHARA. NE. '  * ) . OR. (I . LT. 0) )  GOTO  43 

78 

42 

CONTINUE 
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79 

43 

PROMPT= 1 RACKPOINT  ERROR* 

80 

45 

FTMFLAG2=IOR (FTMFLAG2 ,  KVSKED) 

t 

81 

p 

RETURN 

c 

G  -  Go  Command 

82 

50 

NSW=4 

' 

83 

IF ( IAND ( FTMFLAG2 , MVSKED) .EQ.0) 

FTMFLAG=IOR ( FTMFLAG , MOVEDONE ) 

84 

RETURN 

C 

C 

K  -  Keyboard  Guidance  Command 

e 

85 

60 

NSW=2 

86 

PROMPT  «  'ENTER  DIRECTION  ' 

87 

p 

RETURN 

c 

p 

D  -  Display  Command 

88 

70 

DISPT  =  LINE (3: 5) 

89 

p 

RETURN 

c 

p 

C  -  calibrate  command 

90 

80 

IF  (LINE  (3:  3)  .EQ. '&')  GOTO  86 

91 

1=3 

92 

CALL  GETNO  ( LINE  , I , XI ,  CHARA ) 

93 

IF ( (CHARA. NE. ' , ') .OR. (I.LT.0) ) 

GOTO  83 

94 

CALL  GETNO (LINE, I, ¥1, CHARA) 

95 

IF ( ( CHARA. NE. ') .OR. (I.LT.O) ) 

GOTO  83 

96 

CALL  GETNO (LINE, I, Zl, CHARA) 

97 

IF(I.GE.O)  GOTO  83 

98 

BX  =  XI 

99 

BY  =  Y1 

100 

BZ  =  Z1 

101 

RETURN 

102 

83 

PROMPT  =  'CALIBRATE  ERROR' 

103 

RETURN 

104 

86 

BX  =  -DX 

105 

BY  =  -DY 

106 

BZ  =  -DZ 

107 

RETURN 

c 

r* 

Q  -  Quit  command 

108 

90 

CALL  DISPCH ( CHAR (27)) 

109 

CALL  DISPCH (CHAR (42 )  ) 

110 

DO  92  1=1,10 

111 

CALL  DISPCH (CHAR (10)) 

112 

92 

CONTINUE 

113 

CALL  DISPCH (CHAR (13)) 

114 

CALL  DISPCH ('  PROCESSOR  1 

TWO  IN  TERMINATION  LOOP’) 

115 

CALL  DISPCH (CHAR (10)) 

• 

116 

CALL  DISPCH (CHAR (10)) 

117 

CALL  DISPCH (CHAR (13)) 

118 

CALL  DISPCH ( '  ') 

115 

NSW  =  3 

120 

p 

RETURN 
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C 
r 1 

F  -  Force/Torque  feedback  control 

121 

L 

99 

IF (LINE (3: 3) .EQ. ’O’)  THEN 

122 

FTMFLAG=IOR ( FTMFLAG , FTDOIT) 

123 

ELSE 

124 

FTMFLAG=IAND (FTMFLAG, 2 55-FTDOIT) 

125 

ENDIF 

126 

/-i 

RETURN 

L. 

c 

Directional  Control 

127 

100 

CALL  GETDIR(LINE,NDIR, IEND) 

128 

IF(IEND.NE.O)  THEN 

129 

NSW=1 

130 

PROMPT  =  ‘ENTER  COMMAND' 

131 

RETURN 

132 

ENDIF 

133 

IF  (NDIR)  130,120,110 

134 

120 

RETURN 

135 

130 

ROBPT1 ( -NDIR) =ROBPTl ( -NDIR) +DEL ( -NDIR) 

136 

ROBPT2 (-NDIR) =ROBPT2 (-NDIR) +DEL(-NDIR) 

137 

GOTO  140 

138 

110 

ROBPT1 (NDIR) =ROBPTl (NDIR) -DEL(NDIR) 

139 

ROBPT2 (NDIR) =R0BPT2 (NDIR) -DEL (NDIR) 

140 

140 

FTMFLAG=IOR ( FTMFLAG , NAVSW) 

141 

p 

RETURN 

142 

END 
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2 

3 

4 


5= 

6= 

7= 

8= 

9= 

10= 

11 

12 

13 

14 

15 

16 

17 

18 

19= 

20 

21 

22 

23 

24 


SUBROUTINE  GETDIR ( LINE , NDIR , LEAVE ) 

C  Getp  the  motion  directly  from  ke^garg  input 
c 

CHARACTER* (*)  LINE 

CHARACTER*].  DIRKEY(  18),  CHAN  ,  , 

PATA  DIRP!Y/'l'i,r2?,«3l,f4'/'5l,  6!,?Q‘,!W?,  E  ,  R., 

1  'T1,  !  Y ' ,  1  <I,,,w,,,e,,,r,,,t!,:Y'/ 

C  '  . 

$INCLUDE  (FTORQUE.INC) 

=1  $NOLIST 


1 
1 
1 
i 
i 
i 

1  c 

l 

=i 

=i 

=i 

=i 

=1  c 
=i 

=i 

=i 

=i 

l 

l 

l 

l 

i 

l 

l 

l 


25 

26 

27 

28 

29 

30 

31 
3? 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 


INTEGER  FTMFLAG,FTDOIT,FTIX)NE,E|T^|,|gF^?,ROED?El^|0|D.IF2 

INTEGER  FTGRIP , R2MSG , GRIPPD , STARTD , R1MSG , EC^pC , AUTO , FTAU I . 

INTEGER  MVSKED ) MVBAK , MVRAK 

CHARACTER*  4  0  PROMPT 

CHARACTER*?  DISPT 

LOGICAL*!  RESET, FTPLOT 

PARAMETER  (R2MSG=2  ,R1MSG=?1) 

PARAMETER  (AUTO=34 , GRIPPD=20, STARTDr=65) 

PARAMETER  ( FSTART=1 ,  FTAUTO=2 ,  FTGRl|>f  £ ,  F|DQNE|? ) 

PARAMETER  (FTDpiT=16  ,NAVSW=32  ,MQVEDONE=64  ,RCALC-128) 
pjU*RMETER  (MVSKED=1,MVBAK=2 (MVRAK-4) 

COMMON/FTCOM/FORCE(14) ,FTBIAS(14) ,FTDIF(I4)  FSp£LE,TSCALE,TDZ 
COMMON/FTSUM/F1ZSUM (6) ,F2ZSUM(6) ,FZSUM(6) 

COMMON/FTMAT/XGAIN,YGAIN, ZGAIN(ALENTH,FTMAT? (6, 6) , 

COMMON^XFORM/CTTOWl (3^3) , CTT0^2 (3,?) ^TOOL2 (6,6) , 

FTQOL1 ( 3 ) , FTOOL2 (3), MTOOL1 ( 3 ) , M|QQL2 ( 3 ) , CjTQQLl (3,3) 
CPMMON/VRCT/DX, DY , DZ , DX2 , DY2 , PHIZ 2 , DRHIM 
COMMON/MSGCOM/PROMPT , RESET , FTPLOT ( ?  ? ) 

COMMON/MCNTL/IOUTER , IFLAG , DISPT , F^F^,  FTMFL|G?_ 
COMMQN/NAYECT/BX , BY , BZ , RACKET ( 4  ,  ?>  ?^PT1(6)  ;5?^R0BWDXF2  (6)  , 
CQMMON/NAVEPT/ROBDIF1 ( 6 ) ,ROBDIF2 (6) ,R0RWDIF1 (6) (ROBWDIF  IP} / 

.  RDMAX , RTMAX , NCALC 
CHAR=LINE(l!l) 

LEAV E=0  ,  tvv 

IF( (CHAR.EQ. 'Z') .OR. (CKAR.EQ. ! z ? ) )  THEN 


RETURN 

ENDIF 

DO  10  1=1,1? 

IF  ( CHAR . EQ . DIRKEY ( I ) )  GOTO  20 
10  CONTINUE 

PROMPT  =  'BAD  DIRECTION  KEY  * 

NDIR=Q 

RETURN 

20  IF(I.GT.12)  1=1-6 

IF(I.GT. 6}  THEN 
NDIR=I-6 
ELSE 
NDIR=-I 

ENDIF  58 
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43  PROMPT  =  'ENTER  DIRECTION  • 

44  RETURN 

45  END 
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1 


2 

3 

4 

5 


=1 

6=1 

7=1 

8=1 

9=1 

10=1 

11=1 

=1 


SUBROUTINE  GETNO ( LINE ,  N ,  A ,  E CHAR ) 
C 

C  Translates  Number  from  input  line 
C 


c 

c 

Inputs 

c 

LINE 

(Character) 

c 

c 

N 

( Integer) 

c 

c 

Outputs 

c 

A 

(Real) 

c 

c 

ECHAR 

(Character) 

Input  line  to  parse 

Starting  position  in  input  line 


Number  returned 

Delimiting  character  at  end  of  line 


CHARACTER* (*)  LINE 
CHARACTER* 3 5  TEMP 
CHARACTER* 1  NOS (13) , ECHAR 

DATA  NOS/ ,l'>,2','3'/'4l,'5')'6,,l7','8,,'9'/,0,) 

1 

C 

$INCLUDE  (FTORQUE. INC) 

$NOLIST 


INTEGER  FTMFLAG ,  FTDOIT ,  FTDONE ,  FSTART ,  FTMFLAG2  ,  ROBDIF1 ,  ROBDIF2 

INTEGER  FTGRIP,  R2MSG ,  GRIPPD,  STARTD ,  R1MSG ,  FCALC ,  AUTO ,  FTAUTO 

INTEGER  MVSKED , MVBAK , MVRAK 

CHARACTER* 40  PROMPT 

CHARACTER* 3  DISPT 

LOGICAL*!  RESET, FTPLOT 


12=1 
13=1 
14=1 
15=1 
16=1 
=1  C 


PARAMETER  (R2MSG=2 , R1MSG=1) 

PARAMETER  (AUTO=34 , GRI PPD= 2  0 , S TARTD= 65) 

PARAMETER  (FSTART=1 , FTAUTO=2 , FTGRIP=4 , FTDONE=8 ) 
PARAMETER  (FTDOIT=16 ,  NAVSW=32 ,  MOVEDONE=64 ,  FCALC=12  8 ) 
PARAMETER  (MVSKED=1 , MVBAK=2 , MVRAK=4 ) 


17=1 

18=1 

19=1 

=1 

20=1 

=1 

21=1 

22=1 

23=1 

24=1 

25=1 

=1 

C 


COMMON/ FTCOM/ FORCE ( 14 )  , FTBIAS ( 14 )  ,  FTDIF ( 14 ) , FSCALE , TSCALE , TDZ 
COMMON/FTSUM/F1ZSUM ( 6 ) , F2ZSUM(6)  ,  FZSUti(6) ,FSUM(6) 
COMMON/FTMAT/XGAIN , YGAIN, ZGAIN, ALENTH , FTMAT2 (6,6), 

1  FTMATX (6,6) , RSCALE ( 6 ) 

COMMON/XFORM/ CTTOW1 (3,3), CTT0W2 (3,3), GT00L2 (6,6), 

4  FT00L1 ( 3 ) t FT00L2 (3) ,MT00L1(3) ,MTOOL2(3) ,GT00L1(3,3) 

COMMON/ VECT/ DX, DY, DZ ,  DX2 ,  DY2  ,  PHIZ 2  ,  DPHIM 
COMMON/MSGCOM/PROMPT ,  RESET ,  FTPLOT  (28) 

COMMON/MCNTL/IOUTER ,  IFLAG ,  DISPT ,  FTMFLAG ,  FTMFLAG2 
COMMON/NAVECT/BX , BY , BZ , RACKPT (4,2),  ROBPT1 ( 6 ) , ROBPT2 ( 6 ) 
COMMON/NAVEPT/ROBDIF1 (6) ,ROBDIF2(6)  ,R0BWDIF1(6) ,ROBWDIF2(6)  , 

1  RDMAX , RTMAX , NCALC 


26 

A=0 . 0 

27 

IF(N.LT.O)  RETURN 

C 

28 

ANO=0 . 0 

29 

APLUS=1 . 0 

30 

ADOT=0 . 0 

31 

DO  10  I=N, 50 

32 

ECHAR=LINE(I:I) 
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33 

34 

35 

36 

37  11 

38 

39  12 

40 

41 

42 

43 

44 

45 

46 

47 

48 

49 

50 

51  10 

52 

C 

C  Return  from  the  middle  of  the  line 
C 


53 

100 

A=ANO*APLUS 

54 

N=I+1 

55 

IF(ECHAR.NE. '  ')  RETURN 

56 

DO  105  I=N, 50 

57 

IF(ICHAR(LINE(I:I) ) .EQ.13)  GOTO  110 

58 

IF(LINE(I:I) .NE. '  ')  GOTO  103 

59 

105 

CONTINUE 

60 

103 

N=I 

61 

RETURN 

C 

C  Return  after  carriage  return 

C 

62 

110 

A=ANO*APLUS 

63 

N=-l 

64 

RETURN 

c 

c 

Error  return 

c 

65 

1000 

PROMPT= 1  BAD  NUMBER' 

66 

N=-l 

67 

RETURN 

68 

END 

IF(ICHAR(ECHAR) .EQ.13)  GOTO  110 

IF( (ECHAR.EQ. * , * ) .OR. (ECHAR.EQ. '  '))  GOTO  100 

DO  11  J=l, 14 

IF  (ECHAR.EQ. NOS (J) )  GOTO  12 
CONTINUE 
GOTO  1000 
IF(J.LE.IO)  THEN 
IF(J.EQ.IO)  J=0 
IF(ADOT.EQ.O.O)  THEN 
ANO=10.0*ANO+J 
ELSE 

ANO=ANO+ADOT*J 
ADOT=ADOT/10 . 0 
ENDIF 
ELSE 

IF  (J.EQ.ll)  ADOT= . 1 
IF  (J. EQ.13)  APLUS=-1 . 0 
ENDIF 
CONTINUE 
GO  TO  1000 
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1  SUBROUTINE  TOOLT2 (RX,RY,N) 

2  PINENSION  FTMATB (6,6) 

C 

$INCLPDE  (FTORQUE. INC) 

=1  $NOLIST 

3=1  INTEGER  FTMFLAG, FTDOIT , FTDONE , FSTART , FTMFLAG2 , ROBDIF1 , ROBDIF2 

4=1  INTEGER  FTGRIP , R2MSG , GRIPPD , STARTD , R1MSG , FCALC , AUTO , FTAUTO 

5=1  INTEGER  MVSKED, MVBAK,MVRAK 

6=1  CHARACTER* 40  PROMPT 

7=1  CHARACTER* 3  DISPT 

8=1  LOGICAL* 1  RESET, FTPLOT 

=1  C 

9=1  PARAMETER  (R2MSG=2 , R1MSG=1) 

10=1  PARAMETER  (AUT0=34,GRIPPD=20,STARTD=65) 

11=1  PARAMETER  (FSTART=1 , FTAUTO=2 , FTGRIP=4 , FTDONE=8 ) 

12=1  PARAMETER  (FTDOIT=16,NAVSW=32,MOVEDONE=64,FCALC=128) 

13=1  PARAMETER  (MVSKED=1,MVBAK=2,MVRAK=4) 

=1  C 

14=1  COMMON/FTCOM/FORCE ( 14 ) ,FTBIAS(14) ,FTDIF(14) , FSCALE , TSCALE , TDZ 

15=1  COMMON / FTSUM/ F 1 Z SUM ( 6 ) , F2ZSUM( 6}  ,  FZSUM (6) ,FSUM(6) 

16=1  COMMON/FTMAT/XGAIN , YGAIN , ZGAIN , ALENTH , FTMAT2 (6,6), 

=1  1  FTMATX (6,6) , RSCALE ( 6 ) 

17=1  COMMON/XFORM/ CTTOW1 (3,3) ,CTTOW2 (3,3) ,GTOOL2  (6,6) , 

=1  4  FTOOL1 ( 3 ) ^  FTOOL2 ( 3 ) , MTOOL1 ( 3 ) , MTOOL2 ( 3 ) , GTOOL1  (3,3) 

18=1  COMMON/VECT/DX, DY, DZ, DX2  ,  DY2 , PHIZ 2 , DPHIM 

19=1  COMMON/MSGCOM/ PROMPT , RESET , FTPLOT (28) 

20=1  COMMON /MCNTL/ I OUTER , I FLAG, DISPT, FTMFLAG, FTMFLAG2 

21=1  COMMON/NAVECT/BX , BY , BZ , RACKPT (4,2), ROBPT1 ( 6 ) , ROBPT2 ( 6 ) 

22=1  COMMON/NAVEPT/ROBDIF1 (6) ,ROBDIF2 (6) ,ROBWDIFl(6) ,ROBWDIF2 (6) , 

=1  1  RDMAX , RTMAX , NCALC 

C 


23 

IF  (N.LE.0)  THEN 

24 

IF  (N.LT.0)  THEN 

25 

PHIZ2=Q . 0 

26 

ELSE 

27 

PHIZX=ATAN2 (RX,RY) 

28 

DPHI=PHIZX-PHIZ2 

29 

CALL  LIMZ(DPHI, DPHIM) 

30 

PHIZ2=PHIZ2+DPHI 

31 

ENDIF 

32 

C 

ENDIF 

33 

CZ=COS (PHIZ2) 

34 

C 

SZ=SIN (PHIZ2) 

35 

DO  20  1=1,6 

36 

DO  20  J=l, 6 

37 

20 

GTOOL2 (I , J) =0 , 0 

C 

38 

DO  30  1=1,2 

39 

GTOOL2 (3*1-2 , 3  *1-2) =CZ 

40 

GTOOL2 (3*1-2 ,3*1-1) =SZ 

41 

GTOOL2 (3*1-1, 3*1-2) =-SZ 

42 

GTOOL2 (3*1-1, 3*1-1) =CZ 

43 

GTOOL2 (3*1, 3*1) =1.0 

44 

30 

CONTINUE 
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45 

DO  35  1=1,6 

46 

DO  35  J=l, 6 

47 

FTMATB ( I , J) =0 . 0 

48 

DO  35  K=l, 6 

49 

FTMATB (I, J) =FTMATB (I, J) +FTMAT2 ( I , K) *GTOOL2 ( 7 - J , 7 -K) 

50 

35 

CONTINUE 

C 

C 

51 

DO  45  1=1,6 

52 

DO  45  J=l, 6 

53 

FTMSUM=0 . 0 

54 

DO  40  K=l, 6 

55 

FTMSUM=FTMSUM+GTOOL2 ( I , K) * FTMATB (K,  J) 

56 

40 

CONTINUE 

57 

45 

FTMATX ( I , J) =FTMSUM 

C 

C 

58 

RETURN 

59 

END 
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1  SUBROUTINE  GAINM 

C 

$INCLUDE  (FTORQUE.INC) 

=1  $NOLIST 

2=1  INTEGER  FTMFLAG , FTDOIT , FTDONE , FSTART, FTMFLAG2 , ROBDIF1 , ROBDIF2 

3=1  INTEGER  FTGRIP , R2MSG , GRIPPD , STARTD, R1MSG , FCALG , AUTO , FTAUTO 

4=1  INTEGER  MVSKED , MVBAK , MVRAK 

5=1  CHARACTER*  4  0  PROMPT 

6=1  CHARACTER* 3  DISPT 

7=1  LOGICAL* 1  RESET, FTPLOT 

=1  C 

8=1  PARAMETER  (R2MSG=2,R1MSG=1) 

9=1  PARAMETER  (AUT0=34,GRIPPD=20,STARTD=65) 

10=1  PARAMETER  (FSTART=1 , FTAUTO=2 , FTGRIP=4 , FTDONE=8 ) 

11=1  PARAMETER  (FTDOIT=16 , NAVSW=32 ,MOVEDONE=64 , FCALC=128) 

12=1  PARAMETER  (MVSKED=1,MVBAK=2,MVRAK=4) 

=1  C 

13=1  COMMON/FTCOM/FORCE ( 14 )  ,  FTBIAS ( 14 ) , FTDIF ( 14 ) , FSCALE , TSCALE , TDZ 

14=1  COMMON/FTSUM/F1ZSUM ( 6 ) ,F2ZSUM(6) ,FZSUM(6) ,FSUM(6) 

15=1  COMMON/ FTMAT/XGAIN , YGAIN, ZGAIN, ALENTH, FTMAT2 (6,6), 

=1  1  FTMATX (6,6) , RSCALE ( 6 ) 

16=1  COMMON/XFORM/ CTTOW1 (3,3) ,CTTOW2(3,3) ,GTOOL2(6,6) , 

=1  4  FTOOL1 ( 3 ) , FTOOL2 ( 3 )  ,  MTOOL1 ( 3 ) , MTOOL2 ( 3 ) , GTOOL1 (3,3) 

17=1  COMMON/VECT/DX, DY, DZ , DX2 , DY2 , PHIZ2 , DPHIM 

18=1  COMMON/MSGCOM/PROMPT , RESET, FTPLOT (28 ) 

19=1  COMMON/MCNTL/ IOUTER , I FLAG, DISPT, FTMFLAG, FTMFLAG2 

20=1  COMMON/NAVECT/BX , BY , BZ , RACKPT (4,2) ,ROBPTl(6) ,ROBPT2(6) 

21=1  COMMON/NAVEPT/ROBDIF1 (6) ,ROBDIF2(6) ,ROBWDIFl(6) ,ROBWDIF2(6) , 

=1  1  RDMAX , RTMAX , NC ALC 

C 
C 

C  Set  the  gain  matrix  for  robot  2 
C 

22  FTMAT2 (2 , 5) =YGAIN/20 . 0 

23  FTMAT2 (5,2)=-YGAIN/80.0 

24  FTMAT2 (1,6) =XGAIN 

2  5  FTMAT2 (1,1) =XGAIN/ ALENTH 

2  6  FTMAT2 (6,6) =XGAIN/ ALENTH 

27  FTMAT2 (6,1) =2 . 0*XGAIN/ (ALENTH*ALENTH) 

28  FTMAT2 (3,4) =ZGAIN 

C  FTMAT2 (4 , 3 ) =2 . 0*ZGAIN/ (ALENTH*ALENTH) 

29  RETURN 

30  END 
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1  SUBROUTINE  LIMZ (AVAL, RATE) 

C 

IF (AVAL. GT. 0.0)  THEN 

IF (AVAL . GT . RATE )  AVAL=RATE 
IF (AVAL. LT. .0001)  AVAL=0.0 
ELSE 

I F ( AVAL . LT . -RATE )  AVAL=-RATE 
IF (AVAL. GT. .0001)  AVAL=0.0 
ENDIF 
RETURN 
10  END 
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1  SUBROUTINE  SUBZ(A,BfC) 

2  X=ABS (A-B) 

3  Y=A+. 0005 

4  Y=ABS (Y-B) 

5  Z=A-. 0005 

6  3=ABS (Z-B) 

7  IF( (X.LT. .0001) .OR. (Y.LT. .0001)  .OR.  (Z.LT. .0001) )  THEN 

8  C=0. 0 

9  ELSE 

10  C=A-B 

11  ENDIF 

12  RETURN 

13  END 
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1 

2 

3 

4 

5 

6 

7 

8 
9 

10 

11 

12 

13 


SUBROUTINE  SUBZ(A,B,C) 

X=ABS (A-B) 

Y=A+. 0005 
Y=ABS (Y-B) 

2=A~. 0005 
Z=ABS (Z-B) 

IF((X.LT. .0001) .OR. (Y.LT. .0001)  .OR. (Z.LT, .0001) )  THEN 
C=0. 0 
ELSE 
C=A-B 
ENDIF 
RETURN 
END 
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